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 
26 // copied from an OpenDE demo program
27 void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
28 {
29  int i;
30 
31  if (g == nullptr) 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) pos = dGeomGetPosition (g);
43  if (R == nullptr) R = dGeomGetRotation (g);
44 
45  dVector3 sides;
46  dGeomBoxGetLengths (g,sides);
47  dsDrawBox (pos,R,sides);
48  }
49  else if (type == dSphereClass)
50  {
51  if (pos == nullptr) pos = dGeomGetPosition (g);
52  if (R == nullptr) R = dGeomGetRotation (g);
53  dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
54  }
55  else if (type == dCapsuleClass)
56  {
57  if (pos == nullptr) pos = dGeomGetPosition (g);
58  if (R == nullptr) R = dGeomGetRotation (g);
59  dReal radius,length;
60  dGeomCapsuleGetParams (g,&radius,&length);
61  dsDrawCapsule (pos,R,length,radius);
62  }
63  else if (type == dCylinderClass)
64  {
65  if (pos == nullptr) pos = dGeomGetPosition (g);
66  if (R == nullptr) R = dGeomGetRotation (g);
67  dReal radius,length;
68  dGeomCylinderGetParams (g,&radius,&length);
69  dsDrawCylinder (pos,R,length,radius);
70  }
71  else if (type == dGeomTransformClass)
72  {
73  if (pos == nullptr) pos = dGeomGetPosition (g);
74  if (R == nullptr) R = dGeomGetRotation (g);
75 
76  dGeomID g2 = dGeomTransformGetGeom (g);
77  const dReal *pos2 = dGeomGetPosition (g2);
78  const dReal *R2 = dGeomGetRotation (g2);
79  dVector3 actual_pos;
80  dMatrix3 actual_R;
81  dMULTIPLY0_331 (actual_pos,R,pos2);
82  actual_pos[0] += pos[0];
83  actual_pos[1] += pos[1];
84  actual_pos[2] += pos[2];
85  dMULTIPLY0_333 (actual_R,R,R2);
86  drawGeom (g2,actual_pos,actual_R,0);
87  }
88  else
89  show_aabb = 0;
90 
91  if (show_aabb != 0)
92  {
93  // draw the bounding box for this geom
94  dReal aabb[6];
95  dGeomGetAABB (g,aabb);
96  dVector3 bbpos;
97  for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
98  dVector3 bbsides;
99  for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
100  dMatrix3 RI;
101  dRSetIdentity (RI);
102  dsSetColorAlpha (1,0,0,0.5);
103  dsDrawBox (bbpos,RI,bbsides);
104  }
105 }
106 
107 void DisplayOpenDESpaces::displaySpace(dSpaceID space)
108 {
109  int ngeoms = dSpaceGetNumGeoms(space);
110  for (int i = 0 ; i < ngeoms ; ++i)
111  {
112  dGeomID geom = dSpaceGetGeom(space, i);
113  std::map<dGeomID, Color>::const_iterator it = m_gcolors.find(geom);
114  if (it != m_gcolors.end())
115  dsSetColor(it->second.r,it->second.g,it->second.b);
116  else
117  dsSetColor(m_activeColor.r, m_activeColor.g, m_activeColor.b);
118  drawGeom(geom, nullptr, nullptr, 0);
119  }
120 }
121 
122 void DisplayOpenDESpaces::displaySpaces()
123 {
124  for (unsigned int i = 0 ; i < m_spaces.size() ; ++i)
125  {
126  m_activeColor = m_colors[i];
127  displaySpace(m_spaces[i]);
128  }
129 }
130 
131 void DisplayOpenDESpaces::addSpace(dSpaceID space, float r, float g, float b)
132 {
133  Color c = {r, g, b};
134  m_colors.push_back(c);
135  m_spaces.push_back(space);
136 }
137 
138 void DisplayOpenDESpaces::setGeomColor(dGeomID geom, float r, float g, float b)
139 {
140  Color c = {r, g, b};
141  m_gcolors[geom] = c;
142 }
143 
144 void DisplayOpenDESpaces::clear()
145 {
146  m_spaces.clear();
147  m_colors.clear();
148  m_gcolors.clear();
149 }