HOG2
MotionCaptureMovement.cpp
Go to the documentation of this file.
1 //
2 // MotionCaptureMovement.cpp
3 // hog2 glut
4 //
5 // Created by Nathan Sturtevant on 11/9/13.
6 // Copyright (c) 2013 University of Denver. All rights reserved.
7 //
8 
9 #include "MotionCaptureMovement.h"
10 
11 const double worldSize = 20;
12 const double goalTolerance = 0.5;
13 
14 struct mcData {
15  float turnSpeed;
16  float xOffset, yOffset, zOffset, headingOffset;
17 };
18 
19 const int numActions = 27;
20 
21 mcData d[] = {
22 // {-110.0, -0.911747098, -0.00472318381, 0.563538074, -123.847564697},
23 // {-100.0, -0.869839549, -0.00453251554, 0.668048501, -112.786506653},
24  {-90.0, -0.820025563, -0.00566683663, 0.761402547, -102.513450623},
25  {-80.0, -0.754989743, -0.00520792603, 0.846449375, -90.6535682678},
26  {-70.0, -0.68668443, -0.00474862708, 0.92180711, -79.623046875},
27  {-60.0, -0.621659517, -0.00342348195, 0.979949296, -70.1457858086},
28  {-50.0, -0.556169391, 0.0011247769, 1.02253902, -62.9730377197},
29  {-40.0, -0.492958367, 0.00525747985, 1.06271756, -55.8160095215},
30  {-30.0, -0.373254806, 0.00502402289, 1.10341728, -42.5149187148},
31  {-20.0, -0.233692259, 0.00385937118, 1.13184297, -27.7257080078},
32  {-10.0, -0.106234029, 0.0014725402, 1.12895668, -13.5424880981},
33  {-8.0, -0.0791881531, 0.00153348595, 1.12579584, -10.5965538025},
34  {-6.0, -0.0550608225, 0.0005375818, 1.12165511, -7.847448349},
35  {-4.0, -0.0291669685, 0.000460080802, 1.11724615, -4.96613055468},
36  {-2.0, -0.00520789856, -8.18446206e-005, 1.11239338, -2.18479919434},
37  {0.0 , 0.0199751724, -0.000237718239, 1.10685933, 0.656613349915},
38  {2.0 , 0.0474604815, -0.000476859539, 1.10631061, 3.42151546478},
39  {4.0 , 0.0753834769, -0.000586710812, 1.10548747, 6.21252536774},
40  {6.0 , 0.103622861, -0.000562481582, 1.10425735, 9.0348777771},
41  {8.0 , 0.132121757, -0.000393480062, 1.10258472, 11.8929595947},
42  {10.0, 0.161176234, -0.000701442303, 1.10047519, 14.4690093994},
43  {20.0, 0.305605233, -0.000723838806, 1.07536352, 28.0411071777},
44  {30.0, 0.435892373, 0.00164459657, 1.01915348, 41.6651306152},
45  {40.0, 0.560899138, 0.00394713879, 0.949841857, 53.7326016426},
46  {50.0, 0.64627862, 0.00594675541, 0.905627429, 60.9953536987},
47  {60.0, 0.730919778, 0.00894816965, 0.86073643, 67.0545349121},
48  {70.0, 0.835809886, 0.00966545194, 0.751601994, 78.9754219055},
49  {80.0, 0.922447503, 0.00939706713, 0.622487068, 91.9488887787},
50  {90.0, 0.992749035, 0.00875499006, 0.477440089, 105.611335754},
51 // {100.0, 1.0382669, 0.0079389289, 0.327931404, 118.453689575},
52 // {110.0, 1.04703259, 0.00703916652, 0.195105791, 130.582626343},
53 };
54 
55 void MCEnvironment::GetSuccessors(const mcMovementState &nodeID, std::vector<mcMovementState> &neighbors) const
56 {
57  mcMovementState s;
58  neighbors.resize(0);
59  for (int x = 0; x < numActions; x++)
60  {
61  s = nodeID;
62  ApplyAction(s, x);
63 // s.x = nodeID.x + d[x].zOffset*cos(TWOPI*s.heading/360.0)+d[x].xOffset*sin(TWOPI*s.heading/360.0);
64 // s.y = nodeID.y + d[x].xOffset*cos(TWOPI*s.heading/360.0)+d[x].zOffset*sin(TWOPI*s.heading/360.0);
65 // s.heading = nodeID.heading + d[x].headingOffset;
66  if (s.x > worldSize || s.x < -worldSize)
67  continue;
68  if (s.y > worldSize || s.y < -worldSize)
69  continue;
70  //s.heading += nodeID.heading + d[x].headingOffset;
71 // if (s.heading > 360)
72 // s.heading -= 360;
73 // if (s.heading < 0)
74 // s.heading += 360;
75  neighbors.push_back(s);
76  }
77 }
78 
79 void MCEnvironment::GetActions(const mcMovementState &nodeID, std::vector<mcMovementAction> &actions) const
80 {
81  actions.resize(0);
82  for (int x = 0; x < numActions; x++)
83  {
84  if (nodeID.x + d[x].zOffset*cos(TWOPI*nodeID.heading/360.0)+d[x].xOffset*sin(TWOPI*nodeID.heading/360.0) > worldSize)
85  continue;
86  if (nodeID.x + d[x].zOffset*cos(TWOPI*nodeID.heading/360.0)+d[x].xOffset*sin(TWOPI*nodeID.heading/360.0) < -worldSize)
87  continue;
88  if (nodeID.y + d[x].xOffset*cos(TWOPI*nodeID.heading/360.0)+d[x].zOffset*sin(TWOPI*nodeID.heading/360.0) > worldSize)
89  continue;
90  if (nodeID.y + d[x].xOffset*cos(TWOPI*nodeID.heading/360.0)+d[x].zOffset*sin(TWOPI*nodeID.heading/360.0) < -worldSize)
91  continue;
92 
93 // if ((nodeID.x + d[x].xOffset > worldSize) || (nodeID.x + d[x].xOffset < -worldSize))
94 // continue;
95 // if ((nodeID.y + d[x].yOffset > worldSize) || (nodeID.y + d[x].yOffset < -worldSize))
96 // continue;
97  actions.push_back(x);
98  }
99 }
100 
101 //int GetNumSuccessors(const mcMovementState &stateID) const;
102 mcMovementAction MCEnvironment::GetAction(const mcMovementState &s1, const mcMovementState &s2) const
103 {
104  mcMovementState tmp;
105  for (int x = 0; x < numActions; x++)
106  {
107  GetNextState(s1, x, tmp);
108  if (tmp == s2)
109  return x;
110  }
111  std::vector<mcMovementState> moves;
112  GetSuccessors(s1, moves);
113  assert(false);
114  return 0;
115 // mcMovementAction a;
116 // return a;
117 }
118 
119 void MCEnvironment::ApplyAction(mcMovementState &s, mcMovementAction a) const
120 {
121  // {-160.0, -0.815222442, -0.0184309911, 0.244376302, -152.432628632},
122  s.x += d[a].zOffset*cos(TWOPI*s.heading/360.0)+d[a].xOffset*sin(TWOPI*s.heading/360.0);
123  s.y += d[a].xOffset*cos(TWOPI*s.heading/360.0)+d[a].zOffset*sin(TWOPI*s.heading/360.0);
124  s.heading += d[a].headingOffset;
125  if (s.heading > 360)
126  s.heading -= 360;
127  if (s.heading < 0)
128  s.heading += 360;
129 }
130 
131 void MCEnvironment::GetNextState(const mcMovementState &s1, mcMovementAction a, mcMovementState &s2) const
132 {
133  s2 = s1;
134  ApplyAction(s2, a);
135 }
136 
137 bool MCEnvironment::InvertAction(mcMovementAction &a) const
138 {
139  return false;
140 }
141 
143 double MCEnvironment::HCost(const mcMovementState &node1, const mcMovementState &node2) const
144 {
145  return distance(node1, node2);
146 }
147 
148 double MCEnvironment::GCost(const mcMovementState &node1, const mcMovementState &node2) const
149 {
150  return distance(node1, node2);
151 }
152 
153 double MCEnvironment::GCost(const mcMovementState &node, const mcMovementAction &act) const
154 {
155  mcMovementState node2;
156  GetNextState(node, act, node2);
157  return distance(node, node2);
158 }
159 
160 bool MCEnvironment::GoalTest(const mcMovementState &node, const mcMovementState &goal) const
161 {
162  return (distance(node, goal) < goalTolerance && abs(node.heading-goal.heading) < 5.0);
163 }
164 
165 uint64_t MCEnvironment::GetStateHash(const mcMovementState &node) const
166 {
167  uint64_t x = (node.x+worldSize)*100;
168  uint64_t y = (node.y+worldSize)*100;
169  uint64_t ang = node.heading;
170  return (x<<40)|(y<<20)|(ang);
171 }
172 
173 uint64_t MCEnvironment::GetActionHash(mcMovementAction act) const
174 {
175  return act;
176 }
177 
178 double MCEnvironment::distance(const mcMovementState &n1, const mcMovementState &n2) const
179 {
180  return sqrt((n1.x - n2.x)*(n1.x - n2.x) + (n1.y-n2.y)*(n1.y-n2.y));
181 }
182 
183 //void MCEnvironment::OpenGLDraw() const
184 //{
185 //
186 //}
187 //
188 //void MCEnvironment::OpenGLDraw(const mcMovementState&) const
189 //{
190 //
191 //}
192 //
194 //void MCEnvironment::OpenGLDraw(const mcMovementState&, const mcMovementState&, float) const
195 //{
196 //
197 //}
198 //
199 //void MCEnvironment::OpenGLDraw(const mcMovementState&, const mcMovementAction&) const
200 //{
201 //
202 //}
203 
204 void MCEnvironment::OpenGLDraw(const mcMovementState& oldState, const mcMovementState &newState, float perc) const
205 {
206  int DEG = 360;
207  GLfloat r, g, b, t;
208  GetColor(r, g, b, t);
209 
210  GLdouble xx, yy, zz, rad;
211  GetOpenGLCoord(perc*newState.x + (1-perc)*oldState.x, perc*newState.y + (1-perc)*oldState.y, xx, yy, zz, rad);
212 
213  float rot = (1-perc)*oldState.heading+perc*newState.heading;
214  if ((oldState.heading >= 3*DEG/4) && (newState.heading <= DEG/4))
215  {
216  rot = (1-perc)*oldState.heading+perc*(newState.heading+DEG);
217  if (rot >= DEG)
218  rot -= DEG;
219  }
220  else if ((newState.heading >= 3*DEG/4) && (oldState.heading <= DEG/4))
221  {
222  rot = (1-perc)*(oldState.heading+DEG)+perc*(newState.heading);
223  if (rot >= DEG)
224  rot -= DEG;
225  }
226  GLdouble yoffset = sin(TWOPI*rot/DEG)*rad;
227  GLdouble xoffset = cos(TWOPI*rot/DEG)*rad;
228 
229  glBegin(GL_TRIANGLES);
230  glColor4f(r, g, b/2, t);
231  glVertex3f(xx+xoffset, yy+yoffset, zz);
232  glColor4f(r, g/2, b, t);
233  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
234  glColor4f(r, g, b/2, t);
235  glVertex3f(xx-xoffset+0.5*yoffset, yy-yoffset-0.5*xoffset, zz);
236 
237  glColor4f(r, g/2, b, t);
238  glVertex3f(xx+xoffset, yy+yoffset, zz);
239  glColor4f(r, g, b/2, t);
240  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
241  glColor4f(r, g/2, b, t);
242  glVertex3f(xx-xoffset-0.5*yoffset, yy-yoffset+0.5*xoffset, zz);
243  glEnd();
244 }
245 
246 
247 void MCEnvironment::OpenGLDraw(const mcMovementState &l) const
248 {
249  GLdouble xx, yy, zz, rad;
250  GLfloat r, g, b, t;
251  GetColor(r, g, b, t);
252  GetOpenGLCoord(l.x, l.y, xx, yy, zz, rad);
253 
254  GLdouble yoffset = sin(TWOPI*l.heading/360.0)*rad;//sin(TWOPI*rot/16)*rad;
255  GLdouble xoffset = cos(TWOPI*l.heading/360.0)*rad;//cos(TWOPI*rot/16)*rad;
256 
257  glBegin(GL_TRIANGLES);
258  recVec surfaceNormal;
259  surfaceNormal.x = (((-0.5*xoffset) * (-rad)) - ((+rad) - (-2*yoffset)));
260  surfaceNormal.y = (((rad) * (-2*xoffset)) - ((0.5*yoffset) - (rad)));
261  surfaceNormal.z = (((0.5*yoffset) * (-2*yoffset)) - ((-0.5*xoffset) - (-2*xoffset)));
262  surfaceNormal.normalise();
263  glNormal3f(surfaceNormal.x, surfaceNormal.y, surfaceNormal.z);
264  glColor4f(r, g, b/2, t);
265  glVertex3f(xx+xoffset, yy+yoffset, zz);
266  glColor4f(r, g/2, b, t);
267  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
268  glColor4f(r, g, b/2, t);
269  glVertex3f(xx-xoffset+0.5*yoffset, yy-yoffset-0.5*xoffset, zz);
270 
271  surfaceNormal.x = (((+0.5*xoffset) * (-rad)) - ((+rad) - (-2*yoffset)));
272  surfaceNormal.y = (((rad) * (-2*xoffset)) - ((-0.5*yoffset) - (rad)));
273  surfaceNormal.z = (((-0.5*yoffset) * (-2*yoffset)) - ((+0.5*xoffset) - (-2*xoffset)));
274  surfaceNormal.normalise();
275  glNormal3f(surfaceNormal.x, surfaceNormal.y, surfaceNormal.z);
276  glColor4f(r, g/2, b, t);
277  glVertex3f(xx+xoffset, yy+yoffset, zz);
278  glColor4f(r, g, b/2, t);
279  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
280  glColor4f(r, g/2, b, t);
281  glVertex3f(xx-xoffset-0.5*yoffset, yy-yoffset+0.5*xoffset, zz);
282  glEnd();
283 }
284 
285 
286 void MCEnvironment::OpenGLDraw(const mcMovementState& l, const mcMovementAction &) const
287 {
288  GLdouble xx, yy, zz, rad;
289  GetOpenGLCoord(l.x, l.y, xx, yy, zz, rad);
290  glColor3f(0.5f, 0.5f, 0.5);
291  DrawSphere(xx-rad+l.x, yy-rad+l.y, zz, rad);
292 }
293 
294 
295 void MCEnvironment::GLDrawLine(const mcMovementState &a, const mcMovementState &b) const
296 {
297  GLdouble xx, yy, zz, rad;
298  GetOpenGLCoord(a.x, a.y, xx, yy, zz, rad);
299 
300  GLfloat rr, gg, bb, t;
301  GetColor(rr, gg, bb, t);
302  glColor4f(rr, gg, bb, t);
303 
304  glBegin(GL_LINES);
305  glVertex3f(xx, yy, zz-rad/2);
306  GetOpenGLCoord(b.x, b.y, xx, yy, zz, rad);
307  glVertex3f(xx, yy, zz-rad/2);
308  glEnd();
309 }
310 
311 bool MCEnvironment::GetOpenGLCoord(float x_, float y_, GLdouble &x, GLdouble &y, GLdouble &z, GLdouble &radius) const
312 {
313  x = x_/worldSize;
314  y = y_/worldSize;
315  z = 0;
316  radius = 1.0/worldSize;
317  return true;
318 }
319 
mcData::yOffset
float yOffset
Definition: MotionCaptureMovement.cpp:16
MCEnvironment::ApplyAction
virtual void ApplyAction(mcMovementState &s, mcMovementAction a) const
Definition: MotionCaptureMovement.cpp:119
MCEnvironment::GetStateHash
virtual uint64_t GetStateHash(const mcMovementState &node) const
Definition: MotionCaptureMovement.cpp:165
recVec
A generic vector (essentially the same as a point, but offers normalization)
Definition: GLUtil.h:78
MCEnvironment::OpenGLDraw
virtual void OpenGLDraw() const
Definition: MotionCaptureMovement.h:74
recVec::z
GLdouble z
Definition: GLUtil.h:98
MCEnvironment::GetNextState
virtual void GetNextState(const mcMovementState &, mcMovementAction, mcMovementState &) const
Definition: MotionCaptureMovement.cpp:131
if
if(state==GLUT_DOWN)
Definition: GLUThog.cpp:244
d
mcData d[]
Definition: MotionCaptureMovement.cpp:21
MCEnvironment::GetAction
virtual mcMovementAction GetAction(const mcMovementState &s1, const mcMovementState &s2) const
Definition: MotionCaptureMovement.cpp:102
mcMovementState::x
float x
Definition: MotionCaptureMovement.h:19
DrawSphere
void DrawSphere(GLdouble _x, GLdouble _y, GLdouble _z, GLdouble tRadius)
Definition: GLUtil.cpp:433
mcData::headingOffset
float headingOffset
Definition: MotionCaptureMovement.cpp:16
MCEnvironment::GetActions
virtual void GetActions(const mcMovementState &nodeID, std::vector< mcMovementAction > &actions) const
Definition: MotionCaptureMovement.cpp:79
mcMovementState
Definition: MotionCaptureMovement.h:17
MCEnvironment::GetActionHash
virtual uint64_t GetActionHash(mcMovementAction act) const
Definition: MotionCaptureMovement.cpp:173
mcMovementState::y
float y
Definition: MotionCaptureMovement.h:19
MCEnvironment::GetSuccessors
virtual void GetSuccessors(const mcMovementState &nodeID, std::vector< mcMovementState > &neighbors) const
Definition: MotionCaptureMovement.cpp:55
mcMovementState::heading
float heading
Definition: MotionCaptureMovement.h:20
MCEnvironment::GLDrawLine
void GLDrawLine(const mcMovementState &a, const mcMovementState &b) const
Definition: MotionCaptureMovement.cpp:295
mcMovementAction
int mcMovementAction
Definition: MotionCaptureMovement.h:37
worldSize
const double worldSize
Definition: MotionCaptureMovement.cpp:11
SearchEnvironment< mcMovementState, mcMovementAction >::GetColor
virtual rgbColor GetColor() const
Definition: SearchEnvironment.h:105
mcData
Definition: MotionCaptureMovement.cpp:14
MCEnvironment::InvertAction
virtual bool InvertAction(mcMovementAction &a) const
Definition: MotionCaptureMovement.cpp:137
MCEnvironment::GoalTest
virtual bool GoalTest(const mcMovementState &node, const mcMovementState &goal) const
Definition: MotionCaptureMovement.cpp:160
MCEnvironment::HCost
virtual double HCost(const mcMovementState &node1, const mcMovementState &node2) const
Heuristic value between two arbitrary nodes.
Definition: MotionCaptureMovement.cpp:143
mcData::xOffset
float xOffset
Definition: MotionCaptureMovement.cpp:16
MCEnvironment::GetOpenGLCoord
bool GetOpenGLCoord(float x_, float y_, GLdouble &x, GLdouble &y, GLdouble &z, GLdouble &radius) const
Definition: MotionCaptureMovement.cpp:311
goalTolerance
const double goalTolerance
Definition: MotionCaptureMovement.cpp:12
TWOPI
static const double TWOPI
Definition: GLUtil.h:65
MotionCaptureMovement.h
mcData::turnSpeed
float turnSpeed
Definition: MotionCaptureMovement.cpp:15
recVec::y
GLdouble y
Definition: GLUtil.h:98
MCEnvironment::GCost
virtual double GCost(const mcMovementState &node1, const mcMovementState &node2) const
Definition: MotionCaptureMovement.cpp:148
recVec::x
GLdouble x
Definition: GLUtil.h:98
numActions
const int numActions
Definition: MotionCaptureMovement.cpp:19
recVec::normalise
void normalise()
Normalize a vector.
Definition: GLUtil.cpp:39
MCEnvironment::distance
double distance(const mcMovementState &n1, const mcMovementState &n2) const
Definition: MotionCaptureMovement.cpp:178
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
mcData::zOffset
float zOffset
Definition: MotionCaptureMovement.cpp:16
Generated by   doxygen 1.8.17