displayOpenDE.cpp
1 /*********************************************************************
2  * Rice University Software Distribution License
3  *
4  * Copyright (c) 2010, Rice University
5  * All Rights Reserved.
6  *
7  * For a full description see the file named LICENSE.
8  *
9  *********************************************************************/
10 
11 /* Author: Ioan Sucan */
12 
13 #include "displayOpenDE.h"
14 
15 #ifdef dDOUBLE
16 #define dsDrawBox dsDrawBoxD
17 #define dsDrawSphere dsDrawSphereD
18 #define dsDrawTriangle dsDrawTriangleD
19 #define dsDrawCylinder dsDrawCylinderD
20 #define dsDrawCapsule dsDrawCapsuleD
21 #define dsDrawLine dsDrawLineD
22 #define dsDrawConvex dsDrawConvexD
23 #endif
24 
25 // copied from an OpenDE demo program
26 void DisplayOpenDESpaces::drawGeom(dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
27 {
28  int i;
29 
30  if (g == nullptr)
31  return;
32 
33  if (dGeomIsSpace(g) != 0)
34  {
35  displaySpace((dSpaceID)g);
36  return;
37  }
38 
39  int type = dGeomGetClass(g);
40  if (type == dBoxClass)
41  {
42  if (pos == nullptr)
43  pos = dGeomGetPosition(g);
44  if (R == nullptr)
45  R = dGeomGetRotation(g);
46 
47  dVector3 sides;
48  dGeomBoxGetLengths(g, sides);
49  dsDrawBox(pos, R, sides);
50  }
51  else if (type == dSphereClass)
52  {
53  if (pos == nullptr)
54  pos = dGeomGetPosition(g);
55  if (R == nullptr)
56  R = dGeomGetRotation(g);
57  dsDrawSphere(pos, R, dGeomSphereGetRadius(g));
58  }
59  else if (type == dCapsuleClass)
60  {
61  if (pos == nullptr)
62  pos = dGeomGetPosition(g);
63  if (R == nullptr)
64  R = dGeomGetRotation(g);
65  dReal radius, length;
66  dGeomCapsuleGetParams(g, &radius, &length);
67  dsDrawCapsule(pos, R, length, radius);
68  }
69  else if (type == dCylinderClass)
70  {
71  if (pos == nullptr)
72  pos = dGeomGetPosition(g);
73  if (R == nullptr)
74  R = dGeomGetRotation(g);
75  dReal radius, length;
76  dGeomCylinderGetParams(g, &radius, &length);
77  dsDrawCylinder(pos, R, length, radius);
78  }
79  else if (type == dGeomTransformClass)
80  {
81  if (pos == nullptr)
82  pos = dGeomGetPosition(g);
83  if (R == nullptr)
84  R = dGeomGetRotation(g);
85 
86  dGeomID g2 = dGeomTransformGetGeom(g);
87  const dReal *pos2 = dGeomGetPosition(g2);
88  const dReal *R2 = dGeomGetRotation(g2);
89  dVector3 actual_pos;
90  dMatrix3 actual_R;
91  dMULTIPLY0_331(actual_pos, R, pos2);
92  actual_pos[0] += pos[0];
93  actual_pos[1] += pos[1];
94  actual_pos[2] += pos[2];
95  dMULTIPLY0_333(actual_R, R, R2);
96  drawGeom(g2, actual_pos, actual_R, 0);
97  }
98  else
99  show_aabb = 0;
100 
101  if (show_aabb != 0)
102  {
103  // draw the bounding box for this geom
104  dReal aabb[6];
105  dGeomGetAABB(g, aabb);
106  dVector3 bbpos;
107  for (i = 0; i < 3; i++)
108  bbpos[i] = 0.5 * (aabb[i * 2] + aabb[i * 2 + 1]);
109  dVector3 bbsides;
110  for (i = 0; i < 3; i++)
111  bbsides[i] = aabb[i * 2 + 1] - aabb[i * 2];
112  dMatrix3 RI;
113  dRSetIdentity(RI);
114  dsSetColorAlpha(1, 0, 0, 0.5);
115  dsDrawBox(bbpos, RI, bbsides);
116  }
117 }
118 
119 void DisplayOpenDESpaces::displaySpace(dSpaceID space)
120 {
121  int ngeoms = dSpaceGetNumGeoms(space);
122  for (int i = 0; i < ngeoms; ++i)
123  {
124  dGeomID geom = dSpaceGetGeom(space, i);
125  std::map<dGeomID, Color>::const_iterator it = m_gcolors.find(geom);
126  if (it != m_gcolors.end())
127  dsSetColor(it->second.r, it->second.g, it->second.b);
128  else
129  dsSetColor(m_activeColor.r, m_activeColor.g, m_activeColor.b);
130  drawGeom(geom, nullptr, nullptr, 0);
131  }
132 }
133 
134 void DisplayOpenDESpaces::displaySpaces()
135 {
136  for (unsigned int i = 0; i < m_spaces.size(); ++i)
137  {
138  m_activeColor = m_colors[i];
139  displaySpace(m_spaces[i]);
140  }
141 }
142 
143 void DisplayOpenDESpaces::addSpace(dSpaceID space, float r, float g, float b)
144 {
145  Color c = {r, g, b};
146  m_colors.push_back(c);
147  m_spaces.push_back(space);
148 }
149 
150 void DisplayOpenDESpaces::setGeomColor(dGeomID geom, float r, float g, float b)
151 {
152  Color c = {r, g, b};
153  m_gcolors[geom] = c;
154 }
155 
156 void DisplayOpenDESpaces::clear()
157 {
158  m_spaces.clear();
159  m_colors.clear();
160  m_gcolors.clear();
161 }