forked from arpg/CarPlanner
-
Notifications
You must be signed in to change notification settings - Fork 0
/
GLDebugDrawer.cpp
133 lines (104 loc) · 3.47 KB
/
GLDebugDrawer.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#include "GLDebugDrawer.h"
#include <stdio.h> //printf debugging
GLDebugDrawer::GLDebugDrawer()
:m_debugMode(0)
{
}
void GLDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor)
{
glBegin(GL_LINES);
glColor3f(fromColor.getX(), fromColor.getY(), fromColor.getZ());
glVertex3d(from.getX(), from.getY(), from.getZ());
glColor3f(toColor.getX(), toColor.getY(), toColor.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
}
void GLDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
drawLine(from,to,color,color);
}
void GLDebugDrawer::drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
{
glColor4f (color.getX(), color.getY(), color.getZ(), btScalar(1.0f));
glPushMatrix ();
glTranslatef (p.getX(), p.getY(), p.getZ());
int lats = 5;
int longs = 5;
int i, j;
for(i = 0; i <= lats; i++) {
btScalar lat0 = SIMD_PI * (-btScalar(0.5) + (btScalar) (i - 1) / lats);
btScalar z0 = radius*sin(lat0);
btScalar zr0 = radius*cos(lat0);
btScalar lat1 = SIMD_PI * (-btScalar(0.5) + (btScalar) i / lats);
btScalar z1 = radius*sin(lat1);
btScalar zr1 = radius*cos(lat1);
glBegin(GL_QUAD_STRIP);
for(j = 0; j <= longs; j++) {
btScalar lng = 2 * SIMD_PI * (btScalar) (j - 1) / longs;
btScalar x = cos(lng);
btScalar y = sin(lng);
glNormal3f(x * zr0, y * zr0, z0);
glVertex3f(x * zr0, y * zr0, z0);
glNormal3f(x * zr1, y * zr1, z1);
glVertex3f(x * zr1, y * zr1, z1);
}
glEnd();
}
glPopMatrix();
}
void GLDebugDrawer::drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha)
{
btVector3 halfExtent = (boxMax - boxMin) * btScalar(0.5f);
btVector3 center = (boxMax + boxMin) * btScalar(0.5f);
//glEnable(GL_BLEND); // Turn blending On
//glBlendFunc(GL_SRC_ALPHA, GL_ONE);
glColor4f (color.getX(), color.getY(), color.getZ(), alpha);
glPushMatrix ();
glTranslatef (center.getX(), center.getY(), center.getZ());
glScaled(2*halfExtent[0], 2*halfExtent[1], 2*halfExtent[2]);
// glutSolidCube(1.0);
glPopMatrix ();
//glDisable(GL_BLEND);
}
void GLDebugDrawer::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha)
{
// if (m_debugMode > 0)
{
const btVector3 n=btCross(b-a,c-a).normalized();
glBegin(GL_TRIANGLES);
glColor4f(color.getX(), color.getY(), color.getZ(),alpha);
glNormal3d(n.getX(),n.getY(),n.getZ());
glVertex3d(a.getX(),a.getY(),a.getZ());
glVertex3d(b.getX(),b.getY(),b.getZ());
glVertex3d(c.getX(),c.getY(),c.getZ());
glEnd();
}
}
void GLDebugDrawer::setDebugMode(int debugMode)
{
m_debugMode = debugMode;
}
void GLDebugDrawer::draw3dText(const btVector3& location,const char* textString)
{
}
void GLDebugDrawer::reportErrorWarning(const char* warningString)
{
printf("%s\n",warningString);
}
void GLDebugDrawer::drawContactPoint(const btVector3& pointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)
{
{
btVector3 to=pointOnB+normalOnB*1;//distance;
const btVector3&from = pointOnB;
glColor4f(color.getX(), color.getY(), color.getZ(),1.f);
//glColor4f(0,0,0,1.f);
glBegin(GL_LINES);
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
// glRasterPos3f(from.x(), from.y(), from.z());
// char buf[12];
// sprintf(buf," %d",lifeTime);
//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
}
}