FreeWRL/FreeX3D  3.0.0
Component_RigidBodyPhysics.c
1 /*
2 
3 
4 X3D Rigid Body Physics Component
5 
6 */
7 
8 
9 /****************************************************************************
10  This file is part of the FreeWRL/FreeX3D Distribution.
11 
12  Copyright 2009 CRC Canada. (http://www.crc.gc.ca)
13 
14  FreeWRL/FreeX3D is free software: you can redistribute it and/or modify
15  it under the terms of the GNU Lesser Public License as published by
16  the Free Software Foundation, either version 3 of the License, or
17  (at your option) any later version.
18 
19  FreeWRL/FreeX3D is distributed in the hope that it will be useful,
20  but WITHOUT ANY WARRANTY; without even the implied warranty of
21  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
22  GNU General Public License for more details.
23 
24  You should have received a copy of the GNU General Public License
25  along with FreeWRL/FreeX3D. If not, see <http://www.gnu.org/licenses/>.
26 ****************************************************************************/
27 
28 #include <config.h>
29 #include <system.h>
30 #include <display.h>
31 #include <internal.h>
32 
33 #include <libFreeWRL.h>
34 
35 #ifdef WITH_RBP
36 //#define dSINGLE 1 //Q. do we need to match the physics lib build
37 //#define dDOUBLE 1
38 # include <ode/ode.h>
39 # ifndef dDOUBLE
40 # warning "RigidBodyPhysics: ODE isn't build to use double-precision"
41 # endif
42 #endif
43 
44 #include "../vrml_parser/Structs.h"
45 #include "../vrml_parser/CRoutes.h"
46 #include "../main/headers.h"
47 
48 #include "../world_script/fieldSet.h"
49 #include "../x3d_parser/Bindable.h"
50 #include "Collision.h"
51 #include "quaternion.h"
52 #include "Viewer.h"
53 #include "../opengl/Frustum.h"
54 #include "../opengl/Material.h"
55 #include "../opengl/OpenGL_Utils.h"
56 #include "../input/EAIHelpers.h" /* for newASCIIString() */
57 
58 #include "Polyrep.h"
59 #include "LinearAlgebra.h"
60 //#include "Component_RigidBodyPhysics.h"
61 #include "Children.h"
62 
63 
64 typedef struct pComponent_RigidBodyPhysics{
65  int something;
66 }* ppComponent_RigidBodyPhysics;
67 void *Component_RigidBodyPhysics_constructor(){
68  void *v = MALLOCV(sizeof(struct pComponent_RigidBodyPhysics));
69  memset(v,0,sizeof(struct pComponent_RigidBodyPhysics));
70  return v;
71 }
72 void Component_RigidBodyPhysics_init(struct tComponent_RigidBodyPhysics *t){
73  //public
74  //private
75  t->prv = Component_RigidBodyPhysics_constructor();
76  {
77  ppComponent_RigidBodyPhysics p = (ppComponent_RigidBodyPhysics)t->prv;
78  p->something = 0;
79  }
80 }
81 void Component_RigidBodyPhysics_clear(struct tComponent_RigidBodyPhysics *t){
82  //public
83 }
84 
85 //ppComponent_RigidBodyPhysics p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
86 
87 
88 //Q. do you love criptic macros? Here's a few:
89 int NNC0(struct X3D_Node* node){
90  //NNC Node Needs Compiling
91  return NODE_NEEDS_COMPILING;
92  //return FALSE;
93 }
94 void MNC0(struct X3D_Node* node){
95  //MNC Mark Node Compiled
96  MARK_NODE_COMPILED;
97 }
98 void MNX0(struct X3D_Node* node){
99  //MNX Mark Node Changed
100  node->_change++;
101 }
102 #define NNC(A) NNC0(X3D_NODE(A))
103 #define MNC(A) MNC0(X3D_NODE(A))
104 // #define MNX(A) MNX0(X3D_NODE(A))
105 // #define PPX(A) getTypeNode(X3D_NODE(A)) //possible proto expansion
106 
107 
108 void rbp_run_physics();
109 void set_physics();
110 
111 //#undef WITH_RBP
112 #ifdef WITH_RBP
113 //START MIT LIC >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
114 
115 /*
116 Physics options considered:
117 0. PAL - physics abstraction layer (in c++, not maintained)
118 1. bullet - very popular, c++/no flat-c interface, large
119 2. ode - c api (even though implemented in c++),
120  - maps well to x3d, specs say 'implemented by ode' in one place
121 decision feb 2016: ode
122 
123 X3D
124 http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html
125 
126 ODE
127 http://ode-wiki.org/wiki/index.php?title=Manual:_Rigid_Body_Functions
128 - body, world
129 http://ode-wiki.org/wiki/index.php?title=Manual:_Collision_Detection
130 - geoms, geom postion, geom class, collide(), spaces, contact points
131 
132 X3D ODE
133 CollidableShape geoms
134 CollidableOffset GeomTransform T ?
135  - both: GeomSetPosition,Rotation,Quat,
136 CollisionCollection Geom Class / category bits
137 CollisionSensor dSpaceCollide() collide2() callback()
138 CollisionSpace spaces
139 Contact Contact points
140 RigidBody body
141 RigidBodyCollection world
142 
143 Initializing physics nodes:
144 Freewrl has a few ways of initializing a node:
145 A. during parsing > createX3DNode(), calling a add_ function, like our add_physics()
146  -done before field values are set, so just good for registering existence, not initializing state
147 B. during a render_hier() pass, when 'visiting' a node, detecting if it isn't initialized, or changed, and compiling
148  -advantage: complex node fields will be populated, parents and children will be accessible, modelview transform known
149 C. in startofloopnodeupdates() done once per frame
150 D. after event processing in execution model ie our rbp_run_physics()
151 
152 Of the physics nodes, only CollidableShape and its extra-transform wrapper CollidableOffset need
153 to be visited on a render_hier pass for visual rendering.
154 
155 X3D declared Node Init Field value node types
156 RigidBodyCollection A,D RigidBody
157 RigidBody D CollidableShape, CollidableOffset
158 CollisionCollection D CollidableShape, CollidableOffset, CollisionSpace
159 CollisionSpace D CollidableShape, CollidableOffset, CollisionSpace
160 CollidableShape B,D Shape
161 CollidableOffset B,D CollidableShape
162 CollisionSensor A,D CollisionCollection
163 
164 Generated Node Generating Node
165 CollisionCollection RigidBodyCollection
166 Contact CollisionSensor
167 
168 
169 */
170 
171 //some crap copied & pasted by dug9 from ode's demo_boxstack.cpp demo program:
172 // some constants
173 
174 #define NUM 100 // max number of objects
175 #define DENSITY (1.0) // density of all objects
176 #define GPB 3 // maximum number of geometries per body
177 #define MAX_CONTACTS 8 // maximum number of contact points per body
178 #define MAX_FEEDBACKNUM 20
179 #define GRAVITY REAL(0.5)
180 #define USE_GEOM_OFFSET 1
181 
182 // dynamics and collision objects
183 
184 typedef struct MyObject {
185  dBodyID body; // the body
186  dGeomID geom[GPB]; // geometries representing this body
187 } MyObject;
188 
189 static int num=0; // number of objects in simulation
190 static int nextobj=0; // next object to recycle if num==NUM
191 static dWorldID world = NULL;
192 static dSpaceID space = NULL;;
193 static MyObject obj[NUM];
194 static dJointGroupID contactgroup; //used by nearCallback for per-frame flushable contact joints
195 static int selected = -1; // selected object
196 static int show_aabb = 0; // show geom AABBs?
197 static int show_contacts = 0; // show contact points?
198 static int random_pos = 1; // drop objects from random position?
199 static int write_world = 0;
200 static int show_body = 0;
201 
202 typedef struct MyFeedback {
203  dJointFeedback fb;
204  bool first;
205 }MyFeedback;
206 static int doFeedback=0;
207 static MyFeedback feedbacks[MAX_FEEDBACKNUM];
208 static int fbnum=0;
209 
210 
211 //our registered lists (should be in gglobal p-> or scene->)
212 static struct Vector *x3dworlds = NULL;
213 static struct Vector *x3dcollisionsensors = NULL;
214 static struct Vector *x3dcollisioncollections = NULL; //
215 
216 
217 //I'm not sure what I'll be setting as *data on geom
218 //here's a method to use only one pointer can be either sensor or collection
219 struct X3D_CollidableShape * getCollidableShapeFromData(void *cdata){
220  struct X3D_CollidableShape * xshape = NULL;
221  if(cdata)
222  xshape = (struct X3D_CollidableShape*)cdata;
223  return xshape;
224 }
225 struct X3D_CollisionSensor * getCollisionSensorFromCsensor(void *csensor){
226  struct X3D_CollisionSensor *csens = NULL;
227  if(csensor){
228  switch(X3D_NODE(csensor)->_nodeType){
229  case NODE_CollisionSensor:
230  csens = (struct X3D_CollisionSensor*)csensor;
231  break;
232  case NODE_CollisionCollection:
233  {
234  struct X3D_CollisionCollection *ccol = (struct X3D_CollisionCollection*)csensor;
235  if(ccol->_csensor)
236  csens = (struct X3D_CollisionSensor*)ccol->_csensor;
237  }
238  default:
239  break;
240  }
241  if(csens && !csens->enabled) csens = NULL;
242  }
243  return csens;
244 }
245 struct X3D_CollisionCollection * getCollisionCollectionFromCsensor(void *csensor){
246  struct X3D_CollisionCollection *ccol = NULL;
247  if(csensor){
248  switch(X3D_NODE(csensor)->_nodeType){
249  case NODE_CollisionSensor:
250  {
251  struct X3D_CollisionSensor* csens = (struct X3D_CollisionSensor*)csensor;
252  if(csens->collider)
253  ccol = (struct X3D_CollisionCollection*)csens->collider;
254  }
255  break;
256  case NODE_CollisionCollection:
257  ccol = (struct X3D_CollisionCollection*)csensor;
258  break;
259  default:
260  break;
261  }
262  if(ccol && !ccol->enabled) ccol = NULL;
263  }
264  return ccol;
265 }
266 static struct X3D_Contact static_contacts_p[100];
267 static int static_contacts_n = 0;
268 static struct X3D_Contact *static_contacts_initializer = NULL;
269 static int static_contacts_initialized = FALSE;
270 // this is called by dSpaceCollide when two objects in space are
271 // potentially colliding.
272 // http://ode.org/ode-latest-userguide.html#sec_10_5_0
273 
274 static void nearCallback (void *data, dGeomID o1, dGeomID o2)
275 {
276  if (dGeomIsSpace (o1) || dGeomIsSpace (o2)) {
277  // colliding a space with something
278  dSpaceCollide2 (o1,o2,data,&nearCallback);
279  // collide all geoms internal to the space(s)
280  if (dGeomIsSpace (o1)) {
281  struct X3D_CollisionSpace *cspace = dGeomGetData(o1);
282  if(cspace->enabled)
283  dSpaceCollide ((dSpaceID)o1,data,&nearCallback);
284  }
285  if (dGeomIsSpace (o2)) {
286  struct X3D_CollisionSpace *cspace = dGeomGetData(o2);
287  if(cspace->enabled)
288  dSpaceCollide ((dSpaceID)o2,data,&nearCallback);
289  }
290  } else {
291  int i, numc;
292  void *cdata1, *cdata2;
293  struct X3D_CollidableShape *xshape1, *xshape2;
294  //struct X3D_RigidBody *xbody1, *xbody2;
295  struct X3D_CollisionSensor *xsens1, *xsens2;
296  struct X3D_CollisionCollection *xcol1, *xcol2, *xcol;
297  static int count = 0;
298 
299  dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
300  // if (o1->body && o2->body) return;
301 
302  // exit without doing anything if the two bodies are connected by a joint
303  dBodyID b1 = dGeomGetBody(o1);
304  dBodyID b2 = dGeomGetBody(o2);
305  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
306 
307 
308  //X3D Component_RigidBodyPhysics
309  //somewhere we need to spit out X3DContacts if there was a X3DCollisionSensor
310  cdata1 = dGeomGetData(o1);
311  cdata2 = dGeomGetData(o2);
312  xshape1 = getCollidableShapeFromData(cdata1);
313  xshape2 = getCollidableShapeFromData(cdata2);
314  xcol1 = getCollisionCollectionFromCsensor(xshape1->_csensor);
315  xcol2 = getCollisionCollectionFromCsensor(xshape2->_csensor);
316  xsens1 = getCollisionSensorFromCsensor(xshape1->_csensor);
317  xsens2 = getCollisionSensorFromCsensor(xshape2->_csensor);
318  if(0) if(count < 20){
319  if(xsens1) printf("have csens1 %x\n",xsens1);
320  if(xsens2) printf("have csens2 %x\n",xsens2);
321  if(xcol1) printf("have ccol1 %x\n",xcol1);
322  if(xcol2) printf("have ccol2 %x\n",xcol2);
323  }
324  if(xcol1 && !xcol1->enabled ) return;
325  if(xcol2 && !xcol2->enabled ) return;
326  if(xshape1 && !xshape1->enabled) return;
327  if(xshape2 && !xshape2->enabled) return;
328 
329  count++;
330  xcol = xcol1 ? xcol1 : xcol2; //do we only need one, or how to pick which one?
331 
332  //prep some defaults for any contacts found in dCollide
333  // http://ode.org/ode-latest-userguide.html#sec_7_3_7
334  for (i=0; i<MAX_CONTACTS; i++) {
335  //defaults
336  contact[i].surface.mode = dContactBounce; // | dContactSoftCFM;
337  contact[i].surface.mu = .1; //dInfinity;
338  contact[i].surface.mu2 = 0;
339  contact[i].surface.bounce = .2;
340  contact[i].surface.bounce_vel = 0.1;
341  contact[i].surface.soft_cfm = 0.01;
342 
343  if(xcol){
344  contact[i].surface.mode = xcol->_appliedParametersMask; //dContactBounce; // | dContactSoftCFM;
345  contact[i].surface.mu = xcol->slipFactors.c[0]; //dInfinity;
346  contact[i].surface.mu2 = xcol->slipFactors.c[1];
347  contact[i].surface.bounce = xcol->bounce;
348  contact[i].surface.bounce_vel = xcol->minBounceSpeed;
349  contact[i].surface.soft_cfm = xcol->softnessConstantForceMix;
350  contact[i].surface.soft_erp = xcol->softnessErrorCorrection;
351  contact[i].surface.motion1 = xcol->surfaceSpeed.c[0];
352  contact[i].surface.motion2 = xcol->surfaceSpeed.c[1];
353  }
354  }
355 
356 
357 
358  if (numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,sizeof(dContact)))
359  {
360  const dReal ss[3] = {0.02,0.02,0.02};
361  dMatrix3 RI;
362  dRSetIdentity (RI);
363  for (i=0; i<numc; i++) {
364  dJointID c = dJointCreateContact (world,contactgroup,contact+i);
365  dJointAttach (c,b1,b2);
366  //if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
367 
368  if (doFeedback && (b1==obj[selected].body || b2==obj[selected].body))
369  {
370  if (fbnum<MAX_FEEDBACKNUM)
371  {
372  feedbacks[fbnum].first = b1==obj[selected].body;
373  dJointSetFeedback (c,&feedbacks[fbnum++].fb);
374  }
375  else fbnum++;
376  }
377  if(xsens1 || xsens2){
378  // do we have a collisionSensor node?
379  // If so we need to send out the collisions its looking for
380  int k;
381  struct X3D_Contact *ct;
382  dSurfaceParameters *surface;
383  if(!static_contacts_initialized){
384  //they are nodes, but static. We don't have a pure initialize function
385  //in GeneratedCode (and too rushed to do one now), so will create one non-static,
386  //and use it to initialize the statics
387  static_contacts_initializer = createNewX3DNode0(NODE_Contact);
388  static_contacts_initialized = TRUE;
389  }
390  static_contacts_n++;
391  if(static_contacts_n >= 100) static_contacts_n = 99;
392  k = static_contacts_n -1;
393  ct = &static_contacts_p[k];
394  memcpy(ct,static_contacts_initializer,sizeof(struct X3D_Contact));
395  surface = &contact[i].surface;
396 
397  ct->appliedParameters.p = xcol ? xcol->appliedParameters.p : NULL;
398  ct->appliedParameters.n = xcol ? xcol->appliedParameters.n : 0;
399  ct->_appliedParameters = contact[i].surface.mode;
400  ct->body1 = b1 ? dBodyGetData(b1) : NULL; //we set the user data to X3DRigidBody* when initializing the bodies
401  ct->body2 = b2 ? dBodyGetData(b2) : NULL;
402  ct->bounce = (float)surface->bounce;
403  //double2float(ct->contactNormal.c,contact[i].geom.normal,3);
404  ct->contactNormal.c[0] = contact[i].geom.normal[0];
405  ct->contactNormal.c[1] = contact[i].geom.normal[1];
406  ct->contactNormal.c[2] = contact[i].geom.normal[2];
407 
408  ct->depth = (float)contact[i].geom.depth;
409  ct->frictionCoefficients.c[0] = (float)surface->mu;
410  ct->frictionCoefficients.c[1] = (float)surface->mu2;
411 
412  //double2float(ct->frictionDirection.c,contact[i].fdir1,2);
413  ct->frictionDirection.c[0] = contact[i].fdir1[0];
414  ct->frictionDirection.c[1] = contact[i].fdir1[1];
415  ct->geometry1 = X3D_NODE(xshape1);
416  ct->geometry2 = X3D_NODE(xshape2);
417  ct->minBounceSpeed = (float)surface->bounce_vel;
418  //double2float(ct->position.c,contact[i].geom.pos,3);
419  ct->position.c[0] = contact[i].geom.pos[0];
420  ct->position.c[1] = contact[i].geom.pos[1];
421  ct->position.c[2] = contact[i].geom.pos[2];
422  ct->slipCoefficients.c[0] = (float)surface->mu;
423  ct->slipCoefficients.c[1] = (float)surface->mu2;
424  ct->softnessConstantForceMix = (float)surface->soft_cfm;
425  ct->softnessErrorCorrection = (float)surface->soft_erp;
426  ct->surfaceSpeed.c[0]= (float)surface->motion1;
427  ct->surfaceSpeed.c[1]= (float)surface->motion2;
428  if(xsens1 && xsens1->enabled){
429  //BOMBING - CRoutes was bombing if I was resizing contacts.p and intersections.p in there
430  //so I malloc once to 100=MAXCONTACTS
431  if(xsens1->contacts.p == NULL)
432  xsens1->contacts.p = malloc(100 * sizeof(void*));
433  //if(xsens1->contacts.n == 0)
434  // xsens1->contacts.p = malloc((xsens1->contacts.n+1)*sizeof(void*));
435  //else
436  // xsens1->contacts.p = realloc(xsens1->contacts.p,(xsens1->contacts.n+1)*sizeof(void*));
437  xsens1->contacts.p[xsens1->contacts.n] = X3D_NODE(ct);
438  xsens1->contacts.n++;
439  //we mark these in do_CollisionSensor if contacts.n > 0
440  //MARK_EVENT(X3D_NODE(xsens1),offsetof(struct X3D_CollisionSensor,contacts));
441  //xsens1->isActive = TRUE;
442  //MARK_EVENT(X3D_NODE(xsens1),offsetof(struct X3D_CollisionSensor,isActive));
443  if(xsens1->intersections.p == NULL)
444  xsens1->intersections.p = malloc(100 * sizeof(void*));
445  //if(xsens1->intersections.n == 0)
446  // xsens1->intersections.p = malloc((xsens1->intersections.n+2)*sizeof(void*));
447  //else
448  // xsens1->contacts.p = realloc(xsens1->intersections.p,(xsens1->intersections.n+1)*sizeof(void*));
449  xsens1->intersections.p[xsens1->intersections.n] = X3D_NODE(xshape1);
450  xsens1->intersections.n++;
451 
452  }
453  if(xsens2 && xsens2->enabled && (xsens2 != xsens1 || (xsens1 && !xsens1->enabled))){
454  if(xsens2->contacts.p == NULL)
455  xsens2->contacts.p = malloc(100 * sizeof(void*));
456  //if(xsens2->contacts.n == 0)
457  // xsens2->contacts.p = malloc((xsens2->contacts.n+1)*sizeof(void*));
458  //else
459  // xsens2->contacts.p = realloc(xsens2->contacts.p,(xsens2->contacts.n+1)*sizeof(void*));
460  xsens2->contacts.p[xsens2->contacts.n] = X3D_NODE(ct);
461  xsens2->contacts.n++;
462  //MARK_EVENT(X3D_NODE(xsens2),offsetof(struct X3D_CollisionSensor,contacts));
463  //xsens2->isActive = TRUE;
464  //MARK_EVENT(X3D_NODE(xsens2),offsetof(struct X3D_CollisionSensor,isActive));
465  }
466  if(xsens2 && xsens2->enabled){
467  if(xsens2->intersections.p == NULL)
468  xsens2->intersections.p = malloc(100 * sizeof(void*));
469  //if(xsens2->intersections.n == 0)
470  // xsens2->intersections.p = malloc((xsens2->intersections.n+2)*sizeof(void*));
471  //else
472  // xsens2->contacts.p = realloc(xsens2->intersections.p,(xsens2->intersections.n+1)*sizeof(void*));
473  xsens2->intersections.p[xsens2->intersections.n] = X3D_NODE(xshape2);
474  xsens2->intersections.n++;
475  }
476  }
477  }
478  }
479  }
480 }
481 
482 static int init_rbp_once = 0;
483 //static dThreadingImplementationID threading;
484 //static dThreadingThreadPoolID pool;
485 int init_rbp(){
486  //we don't want to run physics if the scene has no physics stuff
487  if(!init_rbp_once && world && space && contactgroup){
488  init_rbp_once = TRUE;
489  //c++:
490  dInitODE2(0);
491  memset (obj,0,sizeof(obj));
492 
493  //if(0){
494  //dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
495  //dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(4, 0, dAllocateFlagBasicData, NULL);
496  //dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);
498  //dWorldSetStepThreadingImplementation(world, dThreadingImplementationGetFunctions(threading), threading);
499  //}
500  //if(0) dAllocateODEDataForThread(dAllocateMaskAll);
501 
502  }
503  return init_rbp_once;
504 }
505 void finish_rbp(){
506  if(init_rbp_once){
507 
508  //dThreadingImplementationShutdownProcessing(threading);
509  //dThreadingFreeThreadPool(pool);
510  //dWorldSetStepThreadingImplementation(world, NULL, NULL);
511  //dThreadingFreeImplementation(threading);
512 
513 
514  dJointGroupDestroy (contactgroup);
515  dSpaceDestroy (space);
516  dWorldDestroy (world);
517  dCloseODE();
518  init_rbp_once = 0;
519  }
520 
521 }
522 enum {
523 FORCEOUT_ALL = 0xFFFFFFFF,
524 FORCEOUT_NONE = 0x00000000,
525 //MOTOR
526 FORCEOUT_motor1Angle = 1 << 0,
527 FORCEOUT_motor1AngleRate= 1 << 1,
528 FORCEOUT_motor2Angle = 1 << 2,
529 FORCEOUT_motor2AngleRate= 1 << 3,
530 FORCEOUT_motor3Angle = 1 << 4,
531 FORCEOUT_motor3AngleRate= 1 << 5,
532 //DOUBLEAXISHING
533 FORCEOUT_body1AnchorPoint=1 << 6,
534 FORCEOUT_body1Axis = 1 << 7,
535 FORCEOUT_body2AnchorPoint=1 << 8,
536 FORCEOUT_body2Axis = 1 << 9,
537 FORCEOUT_hinge1Angle = 1 << 10,
538 FORCEOUT_hinge1AngleRate= 1 << 11,
539 FORCEOUT_hinge2Angle = 1 << 12,
540 FORCEOUT_hinge2AngleRate= 1 << 13,
541 //SINGLEAXIS
542 FORCEOUT_angle = 1 << 14,
543 FORCEOUT_angleRate = 1 << 15,
544 //SLIDER
545 FORCEOUT_separation = 1 << 16,
546 FORCEOUT_separationRate = 1 << 17,
547 
548 
549 };
550 static struct force_output_fieldname {
551 char *fieldname;
552 unsigned int bitmask;
553 } force_output_fieldnames [] = {
554 {"ALL", FORCEOUT_ALL},
555 {"NONE", FORCEOUT_NONE},
556 //MOTOR
557 {"motor1Angle", FORCEOUT_motor1Angle},
558 {"motor1AngleRate", FORCEOUT_motor1AngleRate},
559 {"motor2Angle", FORCEOUT_motor2Angle},
560 {"motor2AngleRate", FORCEOUT_motor2AngleRate},
561 {"motor3Angle", FORCEOUT_motor3Angle},
562 {"motor3AngleRate", FORCEOUT_motor3AngleRate},
563 //DOUBLEAXISHINGE
564 {"body1AnchorPoint" ,FORCEOUT_body1AnchorPoint},
565 {"body1Axis" ,FORCEOUT_body1Axis},
566 {"body2AnchorPoint" ,FORCEOUT_body2AnchorPoint},
567 {"body2Axis" ,FORCEOUT_body2Axis},
568 {"hinge1Angle" ,FORCEOUT_hinge1Angle},
569 {"hinge1AngleRate" ,FORCEOUT_hinge1AngleRate},
570 {"hinge2Angle" ,FORCEOUT_hinge2Angle},
571 {"hinge2AngleRate" ,FORCEOUT_hinge2AngleRate},
572 //SINGLE
573 {"angle" ,FORCEOUT_angle},
574 {"angleRate" ,FORCEOUT_angleRate},
575 //SLIDER
576 {"separation" ,FORCEOUT_separation},
577 {"separationRate" ,FORCEOUT_separationRate},
578 
579 
580 
581 {NULL,0}
582 };
583 unsigned int forceout_from_names(int n, struct Uni_String **p){
584  int i,j;
585  unsigned int ret = 0;
586  if(!strcmp(p[0]->strptr,"ALL")) return FORCEOUT_ALL;
587  if(!strcmp(p[0]->strptr,"NONE")) return FORCEOUT_NONE;
588  for(i=0;i<n;i++){
589  //struct force_output_fieldname *fname;
590  j=0;
591  do{
592  if(!strcmp(p[i]->strptr,force_output_fieldnames[j].fieldname)){
593  ret |= force_output_fieldnames[j].bitmask;
594  }
595  j++;
596  }while(force_output_fieldnames[j].fieldname != NULL);
597  }
598  return ret;
599 }
600 
601 
602 void setTransformsAndGeom_E(dSpaceID space, void *csensor, struct X3D_Node* parent, struct X3D_Node **nodes, int n){
603  // ATTEMPT 6
604  // this is an initialzation step function, called once for program/scene run, not called again once _body is intialized
605  // USING OCTAGA CONVENTION - only use initial CollidableOffset for offset
606  // which initCollidable() copies to _initialTranslation, _initialRotation just once,
607  // - and zeros the regular translation, rotation for both CollidableOffset and CollidableShape
608  // - either on compile_CollidableXXXX or in run_rigid_body() on initialization of _body
609  // we can recurse if collidableOffset, although not branching recursion
610  // - collidableShape is the leaf
611  // http://ode.org/ode-latest-userguide.html#sec_10_7_7
612  // geomTransform is used between RigidBody and shape geom so geom can have composite delta/shift/offset
613  // see demo_boxstack 'x' keyboard option, which is for composite objects.
614  // phase I - get mass, collision geom, and visual to show up in the same place
615  // phase II - harmonize with RigidBodyCollection->collidables [CollisionCollection] stack, which can
616  // include static geometry not represented as RigidBodys. By harmonize I mean
617  // - detect if already generated collidable, and add RB mass
618 
619  int kn;
620  for(kn=0;kn<n;kn++){
621  dGeomID gid = NULL; //top level geom
622  struct X3D_Node *node = nodes[kn];
623  if(node)
624  if(node->_nodeType == NODE_CollidableShape || node->_nodeType == NODE_CollidableOffset || node->_nodeType == NODE_CollisionSpace){
625  float *translation, *rotation;
626  struct X3D_CollidableOffset *collidable = (struct X3D_CollidableOffset *)node;
627  translation = collidable->translation.c;
628  rotation = collidable->rotation.c;
629 
630  switch(node->_nodeType){
631  case NODE_CollidableShape:
632  {
633  struct X3D_CollidableShape *cshape = (struct X3D_CollidableShape *)node;
634  gid = cshape->_geom;
635  //printf("handle collidable shape\n");
636  if(!cshape->_geom){
637  struct X3D_Shape *shape = (struct X3D_Shape*)cshape->shape;
638  dGeomID shapegid;
639  gid = dCreateGeomTransform (space); //dSpaceID space);
640  dGeomTransformSetCleanup (gid,1);
641 
642  if(shape && shape->geometry){
643 
644  dReal sides[3];
645  switch(shape->geometry->_nodeType){
646  case NODE_Box:
647  {
648  struct X3D_Box *box = (struct X3D_Box*)shape->geometry;
649  sides[0] = box->size.c[0]; sides[1] = box->size.c[1], sides[2] = box->size.c[2];
650  shapegid = dCreateBox(0,sides[0],sides[1],sides[2]);
651  //printf("shape box\n");
652  }
653  break;
654  case NODE_Cylinder:
655  {
656  struct X3D_Cylinder *cyl = (struct X3D_Cylinder*)shape->geometry;
657  sides[0] = cyl->radius;
658  sides[1] = cyl->height;
659  shapegid = dCreateCylinder(0,sides[0],sides[1]);
660  }
661  break;
662  //case convex - not done yet, basically indexedfaceset or triangleSet?
663  case NODE_TriangleSet:
664  {
665  //see ODE demo_heightfield.cpp
666  int j,k;
667  dTriMeshDataID new_tmdata;
668  struct X3D_TriangleSet *tris = (struct X3D_TriangleSet*)shape->geometry;
669  struct X3D_Coordinate *coord = (struct X3D_Coordinate *)tris->coord;
670  int index_count = coord->point.n;
671  dTriIndex * indices = malloc(index_count*sizeof(dTriIndex));
672  for(j=0;j<index_count/3;j++){
673  for( k=0;k<3;k++)
674  indices[j*3 +k] = j*3 + k;
675  }
676  new_tmdata = dGeomTriMeshDataCreate();
677  dGeomTriMeshDataBuildSingle(new_tmdata, coord->point.p, 3 * sizeof(float), coord->point.n,
678  &indices[0], index_count, 3 * sizeof(dTriIndex));
679 
680  shapegid = dCreateTriMesh(0, new_tmdata, 0, 0, 0);
681  //free(indices); will need to clean up at program end, ODE assumes this and the point.p hang around
682  //printf("shape trimesh\n");
683  }
684  break;
685  case NODE_Sphere:
686  default:
687  {
688  struct X3D_Sphere *sphere = (struct X3D_Sphere*)shape->geometry;
689  sides[0] = sphere->radius;
690  shapegid = dCreateSphere(0,sides[0]);
691  //printf("shape sphere\n");
692  }
693  break;
694  }
695  }
696  cshape->_geom = gid; //we will put trans wrapper whether or not there's an offset parent.
697  cshape->_csensor = csensor;
698  dGeomTransformSetGeom (gid,shapegid);
699  dGeomSetData(gid,cshape); //so on collision nearCallback we know which collisionSensor->contacts to set
700  }
701  }
702  break;
703  case NODE_CollisionSpace:
704  {
705  struct X3D_CollisionSpace *cspace = (struct X3D_CollisionSpace *)node;
706 
707  //recurse to leaf-node collidableShape
708  if(!cspace->_space){
709  cspace->_space = dHashSpaceCreate (space);
710  dGeomSetData(cspace->_space,cspace);
711  }
712  //printf("handle collisionspace\n");
713  setTransformsAndGeom_E(cspace->_space,csensor,X3D_NODE(cspace),cspace->collidables.p,1);
714 
715  }
716  break;
717  case NODE_CollidableOffset:
718  {
719  struct X3D_CollidableOffset *coff = (struct X3D_CollidableOffset *)node;
720  //printf("handle collidableoffset\n");
721  //recurse to leaf-node collidableShape
722  struct X3D_Node *nodelist[1];
723  struct X3D_Node *nodelistitem = X3D_NODE(coff->collidable);
724  nodelist[0] = nodelistitem;
725  setTransformsAndGeom_E(space,csensor,X3D_NODE(coff),nodelist,1);
726  gid = coff->_geom;
727  }
728  break;
729  default:
730  break;
731  }
732  if(gid){
733  switch(parent->_nodeType){
734  case NODE_CollidableOffset:
735  {
736  //snippet from ODE demo_boxstack.cpp cmd == 'x' {} section
737  dMatrix3 Rtx;
738  struct X3D_CollidableOffset *coff = (struct X3D_CollidableOffset *)parent;
739  float *translation, *rotation;
740  translation = coff->_initialTranslation.c;
741  rotation = coff->_initialRotation.c;
742  dGeomSetPosition (gid,translation[0],translation[1],translation[2]);
743  dRFromAxisAndAngle (Rtx,rotation[0],rotation[1],rotation[2],rotation[3]);
744  dGeomSetRotation (gid,Rtx);
745  coff->_geom = gid;
746  //printf("parent collidableoffset\n");
747  }
748  break;
749  case NODE_RigidBody:
750  {
751  struct X3D_RigidBody *rb = (struct X3D_RigidBody *)parent;
752  dGeomSetBody (gid,rb->_body);
753  //printf("parent rigidbody\n");
754  }
755  //fallthrough to transform initializing below
756  case NODE_CollisionSpace:
757  //printf("parent space\n");
758  case NODE_CollisionCollection:
759  //printf("parent collisionCollection\n");
760  case NODE_RigidBodyCollection:
761  {
762  //printf("parent rigidbodycollection\n");
763  if(node->_nodeType == NODE_CollidableShape){
764  //we have a collidable, but we aren't inside a rigidbody
765  //so we want to keep and use any translate/rotate for global placement
766  struct X3D_CollidableShape *cshape2 = (struct X3D_CollidableShape *)node;
767  if(!cshape2->_initialized){
768 
769  float *translation, *rotation, *initialtranslation, *initialrotation;
770  translation = cshape2->translation.c;
771  rotation = cshape2->rotation.c;
772  initialtranslation = cshape2->_initialTranslation.c;
773  initialrotation = cshape2->_initialRotation.c;
774  if(!vecsame3f(initialtranslation,translation)){
775  dGeomSetPosition (gid,translation[0],translation[1],translation[2]);
776  veccopy3f(initialtranslation,translation);
777  }
778  if(!vecsame4f(initialrotation,rotation)){
779  dMatrix3 Rtx;
780  dRFromAxisAndAngle (Rtx,rotation[0],rotation[1],rotation[2],rotation[3]);
781  dGeomSetRotation (gid,Rtx);
782  veccopy4f(initialrotation,rotation);
783  }
784  cshape2->_initialized = TRUE;
785  //printf("initializingB\n");
786  }
787  }
788 
789  }
790  default:
791  break;
792  }
793  }
794  }
795  }
796 }
797 
798 static int rbp_pause = FALSE;
799 static double STEP_SIZE = .02; //seconds
800 /* http://www.ode.org/ode-0.039-userguide.html#ref27
801 3.10. Typical simulation code
802 A typical simulation will proceed like this:
803 1.Create a dynamics world.
804 2.Create bodies in the dynamics world.
805 3.Set the state (position etc) of all bodies.
806 4.Create joints in the dynamics world.
807 5.Attach the joints to the bodies.
808 6.Set the parameters of all joints.
809 7.Create a collision world and collision geometry objects, as necessary.
810 8.Create a joint group to hold the contact joints.
811 9.Loop:
812 1. Apply forces to the bodies as necessary.
813 2. Adjust the joint parameters as necessary.
814 3. Call collision detection.
815 4. Create a contact joint for every collision point, and put it in the contact joint group.
816 5. Take a simulation step.
817 6. Remove all joints in the contact joint group.
818 10.Destroy the dynamics and collision worlds.
819 
820 UPDATING NODES when NODE_NEEDS_COMPILING:
821 Usually when you want to change a parameter on a running physics simulation, you
822 don't want the simulation to completely start over. You want it to carry on. The
823 physics engine holds state for where things are.
824 
825 In freewrl > propagate_events_B() > update_node(toNode); we flag the whole target
826 node as needing a recompile, even when only one field was changed.
827 To tell later, when compile_ recompiling the target node, which fields have changed
828 we change the PERL node struct to have __oldfieldvalue and each time we recompile
829 we save the current value of the field. So next time we compile_ the node,
830 we can see if newfield == __oldfieldvalue, and if not, that field changed - apply the change.
831 When we don't check, its because no harm is done by re-setting the field even if not changed.
832 
833 For RBPhysics nodes, there are lots of fields that can be harmlessly reset in the
834 middle of a simulation.
835 But there are some fields that should only be reset if the field changed
836 ORIC - Only Reset If Changed
837 There's a pattern to them:
838 1. they relate to setting up the initial geometry poses, which are done
839  in global coordinates, not geometry or shape or 'local' coordinates, so once the simm starts
840  a change to these makes a mess
841 2. no need to recompile Joint when child Bodies are recompiled - unless the body node (node* pointer) is different
842  - because we update the physics state sufficiently when compiling the body
843 
844 I'll list (my guess at) the ORIC (Only Reset If Changed) fields here:
845 
846 BallJoint
847 anchorPoint
848 body1,body2
849 
850 CollidableShape,CollidableOffset
851 (recompile like Transform node)
852 x but don't flag parent X3DBody as needing to recompile
853 
854 DoubleAxisHingeJoint
855 anchorPoint
856 axis1,2
857 body1,body2
858 
859 MotorJoint
860 motor1Axis,motor2Axis,motor3Axis
861 axisAngle1,axisAngle2,axisAngle3
862 body1,body2
863 
864 RigidBody
865 centerOfMass
866 finiteRotationAxis
867 linearVelocity
868 angularVelocity
869 orientation
870 position
871 
872 SingleAxisHinge
873 anchorPoint
874 axis
875 body1,body2
876 
877 SliderJoint
878 axis
879 body1,body2
880 
881 UniversalJoint
882 anchorPoint
883 axis1,2,3
884 body1,body2
885 
886 Total: 31 ORIC fields
887 (plus lots of other fields that are wastefully reset on a generic recompile)
888 
889 There are various options/possibilities/strategies for recording and recovering which fields were changed:
890 1. __oldvalue fields for ORIC fields. Works well for scalars, but not MF/.n,.p fields (like forceOutput)
891 2. create a bitflag field, one bit for each field in the node
892  (struct X3D_EspduTransform has 90 fields, and several are in the 30-40 range, would need 3 int32 =12 bytes)
893 3. refactor freewrl code to include a bitflag in each field (like Script and Proto field structs)
894 4. as in 2,3 except node would declare a __bitflag field only if it cares, and update_node would
895  check for that field and set the field bit only if it has the __bitflag field.
896  a) in the default part of the node struct, a pointer field called __bitflag which is normally null
897  - and malloced if needed
898  b) in the node-specific part, an MFInt32 field, and n x 32 bits are available
899 Issue with 2,3,4: either you need all the bitflags set on startup (to trigger all fields compile)
900  or else you need a way to signal when its a partial recompile vs full recompile
901  - perhaps a special _change value on startup.
902 
903 DECISION: we'll do the #1 __oldvalue technique.
904 
905 */
906 int static_did_physics_since_last_Tick = FALSE;
907 void rbp_run_physics(){
908  /* called once per frame.
909  assumes we have lists of registered worlds (x3drigidbodycollections) and
910  registered collisionSensors if any
911  for each world,
912  - we drill down and check each thing and if not initialized, we initialize it
913  - we run the collision and simulation steps
914  - we output to any collisionSensors
915  */
916  int nstep;
917  ppComponent_RigidBodyPhysics p;
918  p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
919 
920  nstep = 1;
921  if(1){
922  //situation: physics simulations need constant size time steps ie STEP_SIZE seconds .02
923  //goal: make the simulation speed match wall clock
924  //method: from render thread, skip or add simulation steps as/when needed,
925  // so average wallclock time between steps ~= stepsize (seconds)
926  //advantage: no thread marshalling needed
927  //versus: separate thread which could sleep, sim, sleep, sim .. to match STEP_SIZE
928  // - disadvantage of separate thread: would need to block threads while moving data between the threads
929  double thistime;
930  static double lasttime;
931  static double remainder = 0.0;
932  static int done_once = 0;
933 
934  thistime = TickTime();
935  if(done_once > 0){
936  double deltime = thistime - lasttime + remainder;
937  nstep = (int) floor( deltime / STEP_SIZE );
938  remainder = deltime - (nstep * STEP_SIZE);
939  }
940  lasttime = thistime;
941  done_once = 1;
942  }
943  //printf("%d ",nstep);
944  if(nstep < 1) return;
945  //see nstep below when calling dworldquickstep we loop over nstep
946  static_did_physics_since_last_Tick = TRUE;
947 
948 
949  if(init_rbp()){
950  int i,j,k;
951  struct X3D_RigidBodyCollection *x3dworld;
952 
953  if(x3dcollisionsensors){
954  struct X3D_CollisionSensor *csens;
955  for(i=0;i<x3dcollisionsensors->n;i++){
956  csens = vector_get(struct X3D_CollisionSensor*,x3dcollisionsensors,i);
957  //clear contacts from last frame
958  csens->contacts.n = 0;
959  // leave at 100 FREE_IF_NZ(csens->contacts.p);
960  //clear intersections from last frame
961  csens->intersections.n = 0;
962  if(csens->collider){
963  //this doesn't run the collision sensor, just makes sure it's 'compiled' / initialized
964  //so no need to check for ->enabled
965  struct X3D_CollisionCollection *ccol = (struct X3D_CollisionCollection *)csens->collider;
966  ccol->_csensor = csens;
967  }
968  }
969  }
970 
971  if(x3dcollisioncollections){
972  // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html#CollisionCollection
973  struct X3D_CollisionCollection *ccol;
974  struct X3D_CollisionSensor *csens;
975  void * csensor;
976  for(i=0;i<x3dcollisioncollections->n;i++){
977  ccol = vector_get(struct X3D_CollisionCollection*,x3dcollisioncollections,i);
978 
979  csens = ccol->_csensor;
980  if(NNC(ccol)){
981  unsigned int mask = 0;
982  int k;
983  for(k=0;k<ccol->appliedParameters.n;k++){
984  //I shamefully copied and pasted enums from ode header without trying to understand them
985  // http://ode.org/ode-latest-userguide.html#sec_7_3_7
986  const char *ap = ccol->appliedParameters.p[k]->strptr;
987  if(!strcmp(ap,"BOUNCE")){
988  mask |= dContactBounce;
989  }else if(!strcmp(ap,"USER_FRICTION")){
990  mask |= dContactFDir1;
991  }else if(!strcmp(ap,"FRICTION_COEFFICIENT-2")){
992  mask |= dContactMu2;
993  }else if(!strcmp(ap,"ERROR_REDUCTION")){
994  mask |= dContactSoftERP;
995  }else if(!strcmp(ap,"CONSTANT_FORCE")){
996  mask |= dContactSoftCFM;
997  }else if(!strcmp(ap,"SPEED-1")){
998  mask |= dContactMotion1;
999  }else if(!strcmp(ap,"SPEED-2")){
1000  mask |= dContactMotion2;
1001  }else if(!strcmp(ap,"SLIP-1")){
1002  mask |= dContactSlip1;
1003  }else if(!strcmp(ap,"SLIP-2")){
1004  mask |= dContactSlip2;
1005  }
1006  }
1007  ccol->_appliedParametersMask = mask;
1008  // Q. how to 'enable=FALSE' a collisionCollection?
1009  // H: ODE category bits? dug9 Dec 2016 I didn't implement.
1010  MNC(ccol);
1011  }
1012  csensor = csens ? (void*)csens : (void*)ccol; //user data assigned to geom, for recovery from collided geoms in nearCallback
1013  setTransformsAndGeom_E(space, csensor, X3D_NODE(ccol), ccol->collidables.p, ccol->collidables.n);
1014  }
1015  }
1016 
1017 
1018  for(j=0;j<vectorSize(x3dworlds);j++){
1019  struct X3D_RigidBody *x3dbody;
1020  struct X3D_CollidableOffset *x3doffset;
1021  struct X3D_CollidableShape *x3dcshape;
1022  x3dworld = (struct X3D_RigidBodyCollection*)vector_get(struct X3D_Node*,x3dworlds,j);
1023  //Collidable -> rigidbody
1024  if(!x3dworld->enabled) continue;
1025  //if(!x3dworld->_world){
1026  if(NNC(x3dworld) || x3dworld->_world == NULL){
1027  x3dworld->_world = world;
1028  if(x3dworld->contactSurfaceThickness > 0)
1029  dWorldSetContactSurfaceLayer (x3dworld->_world, x3dworld->contactSurfaceThickness);
1030  dWorldSetGravity (x3dworld->_world, x3dworld->gravity.c[0], x3dworld->gravity.c[1], x3dworld->gravity.c[2]);
1031  dWorldSetERP (x3dworld->_world, x3dworld->errorCorrection);
1032  dWorldSetCFM (x3dworld->_world, x3dworld->constantForceMix);
1033  dWorldSetAutoDisableFlag (x3dworld->_world, x3dworld->autoDisable);
1034  dWorldSetAutoDisableLinearThreshold (x3dworld->_world, x3dworld->disableLinearSpeed);
1035  dWorldSetAutoDisableAngularThreshold (x3dworld->_world, x3dworld->disableAngularSpeed);
1036  dWorldSetAutoDisableTime (x3dworld->_world, x3dworld->disableTime);
1037  if(x3dworld->maxCorrectionSpeed > -1)
1038  dWorldSetContactMaxCorrectingVel (x3dworld->_world, x3dworld->maxCorrectionSpeed);
1039  MNC(x3dworld);
1040  }
1041 
1042  if(x3dworld->set_contacts.n){
1043  //someone in javascript / sai sent us some extra contacts to add as contact joints
1044  int kk;
1045  for (kk=0; kk < x3dworld->set_contacts.n; kk++) {
1046  struct X3D_RigidBody *body1, *body2;
1047  struct X3D_Contact *ct = (struct X3D_Contact *)x3dworld->set_contacts.p[kk];
1048  dContact contact;
1049  dBodyID b1, b2;
1050  dJointID c = dJointCreateContact (world,contactgroup,&contact);
1051 
1052  contact.surface.mode = ct->_appliedParameters; //dContactBounce; // | dContactSoftCFM;
1053  contact.surface.mu = ct->slipCoefficients.c[0]; //dInfinity;
1054  contact.surface.mu2 = ct->slipCoefficients.c[1];
1055  contact.surface.bounce = ct->bounce;
1056  contact.surface.bounce_vel = ct->minBounceSpeed;
1057  contact.surface.soft_cfm = ct->softnessConstantForceMix;
1058  contact.surface.soft_erp = ct->softnessErrorCorrection;
1059  contact.surface.motion1 = ct->surfaceSpeed.c[0];
1060  contact.surface.motion2 = ct->surfaceSpeed.c[1];
1061  float2double(contact.geom.pos,ct->position.c,3);
1062  float2double(contact.fdir1,ct->frictionDirection.c,2);
1063  float2double(contact.geom.normal,ct->contactNormal.c,3);
1064 
1065  body1 = (struct X3D_RigidBody*)ct->body1;
1066  body2 = (struct X3D_RigidBody*)ct->body2;
1067  b1 = body1 ? body1->_body : NULL;
1068  b2 = body2 ? body2->_body : NULL;
1069  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) continue;
1070  dJointAttach (c,b1,b2);
1071  }
1072  x3dworld->set_contacts.n = 0;
1073  }
1074  //update bodies
1075  for(i=0;i<x3dworld->bodies.n;i++){
1076  int isNewBody = FALSE;
1077  int method_anchor_fixed_with_fixed_joint = 1;
1078  x3dbody = (struct X3D_RigidBody*)x3dworld->bodies.p[i];
1079  if(method_anchor_fixed_with_fixed_joint){
1080  if(!x3dbody->_body){
1081  x3dbody->_body = dBodyCreate (x3dworld->_world);
1082  dBodySetData (x3dbody->_body,(void*) x3dbody);
1083  if(x3dbody->fixed){
1084  //a few joints - maybe just hinge2? - need 2 bodies or ODE ASSERTS.
1085  //anchor with unrecommended fixed joint
1086  //note we still need MASS on the fixed body, otherwise it jitters
1087  dJointID anchorID = dJointCreateFixed(x3dworld->_world,x3dworld->_group);
1088  dJointAttach(anchorID,x3dbody->_body,0);
1089  dJointSetFixed (anchorID);
1090  }
1091  isNewBody = TRUE;
1092  }
1093  }else{
1094  if(!x3dbody->_body && !x3dbody->fixed){
1095  //_body == 0 tells ODE its fixed geometry
1096  x3dbody->_body = dBodyCreate (x3dworld->_world);
1097  dBodySetData (x3dbody->_body,(void*) x3dbody);
1098  isNewBody = TRUE;
1099  }
1100  }
1101 
1102  if(x3dbody->enabled)
1103  dBodyEnable (x3dbody->_body);
1104  else
1105  dBodyDisable(x3dbody->_body);
1106  x3dcshape = NULL;
1107  x3doffset = NULL;
1108  if(isNewBody){
1109  // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html#CollidableOffset
1110  // collidableOffset can recurse ie it's collidable can be either another collidableOffset, or a collidableShape
1111  // however it's not a branching recurse.
1112  // ODE http://ode.org/ode-latest-userguide.html#sec_10_7_7
1113  // ode has a geomTransform
1114 
1115 
1116  // ATTEMPT 6, Dec 8 and 9, 10 2016
1117  //ATTEMPT 6 WILL USE OCTAGA CONVENTION for CollidableOffset
1118  //- ignore CollidableShape pose on init
1119  //- use CollidableOffset pose for compositing offset/delta
1120  //- write to only the top Collidable transform fields
1121  //- apply the offset -if CollidableOffset- before writing the transform fields
1122  //- on render, use only the top level transform (which will include any offset if present)
1123  //- a way to ensure, is when compile_ collideable, save to __initials only if Offset,
1124  // then zero transform without marking event
1125  // based on ODE test demo_boxstack.cpp 'x' option for composite object
1126  // - it seems to be doing within-RB transforms during composition
1127  // - then transforming the rigidbody during physics
1128  // - 2 ways: a) USE_GEOM_OFFSET b) geomTransform
1129  // - and in both cases the composition transforms are applied to the collidable shape geom
1130  // - geomTransform is just there to force separation between RB and collidable
1131  // so they don't share transform
1132  // IDEA:
1133  // 1. stop ODE from transmitting initial x3dbody pose to geom and vice versa
1134  // for the purposes of allowing composite objects, and do that
1135  // by always inserting exactly one geomTransform wrapper per RigidBody
1136  // with no transform applied to the geomTransform
1137  // see ODE demo_boxstack.cpp 'x' option
1138  // 2. when recursing down Collidables stack for first traverse/initialization,
1139  // a) on init of Collidable, save CollidableOffset to initialTranslation,_initialRotation,
1140  // zero the translation,rotation fields of CollidableOffset and CollidableShape
1141  // b) apply only CollidableOffset initial transform (saved in _initialTranslation,_initialRotation)
1142  // to shape, to position it relative to its parent/grandParent RigidBody,
1143  // and apply it only to leaf geom ie box, sphere, ...
1144  // 3. on MARK_EVENT set RigidBody pose and MARK,
1145  // and take RigidBody pose, apply _initialTranslation, _initialRotation of top collidable,
1146  // and set on top collidable* translation,rotation and MARK_EVENT
1147  // 4. on scenegraph traversal
1148  // transform using collidable transforms like a normal transform stack
1149  // This should allow composite geom RigidBodys
1150  // visualization - as usual either expose collidable in scenegraph, or route from them
1151  // to individual parts, or from the corresponding rigidbody to a wrapper transform on other geom
1152  // that doesn't need the individual part transforms
1153  // Phase I - get SingleHingeJoint type scenes to work
1154  // Phase II - harmonize with RigidBodyCollection->collidables [CollidableCollection] scenes
1155 
1156  float *translation, *rotation;
1157  translation = x3dbody->position.c;
1158  rotation = x3dbody->orientation.c;
1159 
1160  for(k=0;k<x3dbody->geometry.n;k++){
1161  //iterate over composite geometry
1162  //Phase I: just do _geom if not already done, and do both collidable geom and mass
1163  // composition in one callstack
1164  //Phase II option: recurse even if _geom set by RBP->collidables separate traverse/callstack
1165  // so we can set mass to the same pose
1166  struct X3D_Node *nodelistitem;
1167  struct X3D_Node *nodelist[1];
1168  struct X3D_CollidableOffset* collidable = (struct X3D_CollidableOffset*)x3dbody->geometry.p[k];
1169  nodelistitem = X3D_NODE(collidable);
1170  nodelist[0] = nodelistitem;
1171  setTransformsAndGeom_E(space,NULL,X3D_NODE(x3dbody), nodelist,1);
1172  }
1173  if(verify_translate(translation)){
1174  //printf("verified translation= %f %f %f\n",translation[0],translation[1],translation[2]);
1175  dBodySetPosition (x3dbody->_body, (dReal)translation[0],(dReal)translation[1],(dReal)translation[2]);
1176  }
1177  if(verify_rotate(rotation)){
1178  //printf("verified reotation= %f %f %f\n",rotation[0],rotation[1],rotation[2]);
1179  if(1){
1180  dReal dquat[4];
1181  Quaternion quat;
1182  vrmlrot_to_quaternion(&quat,rotation[0],rotation[1],rotation[2],rotation[3]);
1183  dquat[0] = quat.w; dquat[1] = quat.x, dquat[2] = quat.y, dquat[3] = quat.z;
1184  dBodySetQuaternion(x3dbody->_body,dquat);
1185  }else{
1186  dMatrix3 R;
1187  dRFromAxisAndAngle (R,rotation[0],rotation[1],rotation[2],rotation[3]);
1188  dBodySetRotation(x3dbody->_body,R);
1189  }
1190  }
1191 
1192  }
1193  if(x3dbody->_body){
1194  //not fixed
1195  if(NNC(x3dbody)){
1196  float speed;
1197  //not fixed
1198  if(x3dbody->autoDamp){
1199  dBodySetAngularDamping(x3dbody->_body, x3dbody->angularDampingFactor);
1200  dBodySetLinearDamping(x3dbody->_body, x3dbody->linearDampingFactor);
1201  }else{
1202  dBodySetDampingDefaults(x3dbody->_body);
1203  }
1204  if(x3dbody->massDensityModel){
1205  dReal sides[3];
1206  dMass m;
1207  switch(x3dbody->massDensityModel->_nodeType){
1208  case NODE_Box:
1209  {
1210  struct X3D_Box *box = (struct X3D_Box*)x3dbody->massDensityModel;
1211  sides[0] = box->size.c[0]; sides[1] = box->size.c[1], sides[2] = box->size.c[2];
1212  dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
1213  }
1214  break;
1215  case NODE_Cylinder:
1216  {
1217  struct X3D_Cylinder *cyl = (struct X3D_Cylinder*)x3dbody->massDensityModel;
1218  sides[0] = cyl->radius;
1219  sides[1] = cyl->height;
1220  dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
1221  }
1222  break;
1223  //case convex - not done yet, basically indexedfaceset
1224  case NODE_Sphere:
1225  {
1226  struct X3D_Sphere *sphere = (struct X3D_Sphere*)x3dbody->massDensityModel;
1227  sides[0] = sphere->radius;
1228  dMassSetSphere (&m,DENSITY,sides[0]);
1229  }
1230  break;
1231  default:
1232  {
1233  // http://ode.org/ode-latest-userguide.html#sec_9_2_0
1234  // 9.2 Mass Functions
1235  float *I = x3dbody->inertia.c;
1236  dMassSetParameters (&m, DENSITY,
1237  0.0, 0.0, 0.0, //center of gravity - we'll adjust below in dMassTranslate
1238  I[0], I[1], I[2], //diagonal elements of 3x3 inertial tensor
1239  I[3], I[4], I[5]); //upper diagonal of 3x3 inertial tensor
1240  }
1241  break;
1242  }
1243  dMassAdjust(&m,x3dbody->mass);
1244  dMassTranslate (&m,x3dbody->centerOfMass.c[0],x3dbody->centerOfMass.c[1],x3dbody->centerOfMass.c[2]);
1245  dBodySetMass (x3dbody->_body, &m);
1246  }else{
1247  dMass m;
1248  dMassSetSphere (&m,DENSITY,.01);
1249  dMassAdjust(&m,x3dbody->mass);
1250  dMassTranslate (&m,x3dbody->centerOfMass.c[0],x3dbody->centerOfMass.c[1],x3dbody->centerOfMass.c[2]);
1251  dBodySetMass (x3dbody->_body, &m);
1252  }
1253  if(x3dbody->useFiniteRotation){
1254  if(!vecsame3f(x3dbody->__old_finiteRotationAxis.c,x3dbody->finiteRotationAxis.c)){
1255  dBodySetFiniteRotationAxis (x3dbody->_body, x3dbody->finiteRotationAxis.c[0],x3dbody->finiteRotationAxis.c[1],x3dbody->finiteRotationAxis.c[2]);
1256  veccopy3f(x3dbody->__old_finiteRotationAxis.c,x3dbody->finiteRotationAxis.c);
1257  }
1258  }
1259  dBodySetFiniteRotationMode (x3dbody->_body, x3dbody->useFiniteRotation ? 1 : 0);
1260 
1261  //gravity?
1262  if(!x3dbody->useGlobalGravity){
1263  dBodySetGravityMode(x3dbody->_body,0);
1264  }
1265  //position and orientation set once in if(!_geom) above
1266  //add any per-step per-body forces
1267  speed = veclength3f(x3dbody->linearVelocity.c);
1268  if(speed > .001f){
1269  if(!vecsame3f(x3dbody->__old_linearVelocity.c,x3dbody->linearVelocity.c)){
1270  dBodySetLinearVel(x3dbody->_body, x3dbody->linearVelocity.c[0],x3dbody->linearVelocity.c[1],x3dbody->linearVelocity.c[2]);
1271  veccopy3f(x3dbody->__old_linearVelocity.c,x3dbody->linearVelocity.c);
1272  }
1273  }
1274  speed = veclength3f(x3dbody->angularVelocity.c);
1275  if(speed > .0001f){
1276  if(!vecsame3f(x3dbody->__old_angularVelocity.c,x3dbody->angularVelocity.c)){
1277  dBodySetAngularVel(x3dbody->_body, x3dbody->angularVelocity.c[0],x3dbody->angularVelocity.c[1],x3dbody->angularVelocity.c[2]);
1278  veccopy3f(x3dbody->__old_angularVelocity.c,x3dbody->angularVelocity.c);
1279  }
1280  }
1281  dBodySetAutoDisableFlag (x3dbody->_body, x3dbody->autoDisable ? 1 : 0);
1282  dBodySetAutoDisableLinearThreshold (x3dbody->_body, x3dbody->disableLinearSpeed);
1283  dBodySetAutoDisableAngularThreshold (x3dbody->_body, x3dbody->disableAngularSpeed);
1284  dBodySetAutoDisableTime (x3dbody->_body, x3dbody->disableTime);
1285  if(x3dbody->forces.n){
1286  //the engine's force accumulator is zeroed after each step, so for these
1287  //constant forces you have to re-add them on each step
1288  int kf;
1289  for(kf=0;kf<x3dbody->forces.n;kf++){
1290  float *force = x3dbody->forces.p[kf].c;
1291  dBodyAddForce(x3dbody->_body, force[0], force[1], force[2]);
1292  }
1293  }
1294  if(x3dbody->torques.n){
1295  //the engine's torque accumulator is zeroed after each step, so for these
1296  //constant torques you have to re-add them on each step
1297  int kf;
1298  for(kf=0;kf<x3dbody->torques.n;kf++){
1299  float *torque = x3dbody->torques.p[kf].c;
1300  dBodyAddTorque(x3dbody->_body, torque[0], torque[1], torque[2]);
1301  }
1302  }
1303  } //if NCC(x3dbody)
1304 
1305  } // if x3dbody->_body (not fixed)
1306 
1307  } //bodies
1308  //update joints
1309  for(i=0;i<x3dworld->joints.n;i++){
1310  struct X3D_Node *joint = (struct X3D_Node*)x3dworld->joints.p[i];
1311  // render_node(joint); //could use virt function? I'll start without virt
1312  switch(joint->_nodeType){
1313  case NODE_BallJoint:
1314  //render_BallJoint(joint);
1315  {
1316  dJointID jointID;
1317  struct X3D_BallJoint *jnt = (struct X3D_BallJoint*)joint;
1318  if(!jnt->_joint){
1319  dBodyID body1ID, body2ID;
1320  struct X3D_RigidBody *xbody1, *xbody2;
1321  xbody1 = (struct X3D_RigidBody*)jnt->body1;
1322  xbody2 = (struct X3D_RigidBody*)jnt->body2;
1323  //allow for MULL body on one side of joint, to fix to static environment
1324  body1ID = xbody1 ? xbody1->_body : NULL;
1325  body2ID = xbody2 ? xbody2->_body : NULL;
1326  jointID = dJointCreateBall (x3dworld->_world,x3dworld->_group);
1327  dJointAttach (jointID,body1ID,body2ID);
1328  jnt->_joint = jointID;
1329  dJointSetBallAnchor(jnt->_joint, jnt->anchorPoint.c[0],jnt->anchorPoint.c[1], jnt->anchorPoint.c[2]);
1330  jnt->_forceout = forceout_from_names(jnt->forceOutput.n,jnt->forceOutput.p);
1331  }
1332  if(NNC(jnt)){
1333  if(!vecsame3f(jnt->__old_anchorPoint.c,jnt->anchorPoint.c)){
1334  dJointSetBallAnchor(jnt->_joint, jnt->anchorPoint.c[0],jnt->anchorPoint.c[1], jnt->anchorPoint.c[2]);
1335  veccopy3f(jnt->__old_anchorPoint.c,jnt->anchorPoint.c);
1336  }
1337  jnt->_forceout = forceout_from_names(jnt->forceOutput.n,jnt->forceOutput.p);
1338  MNC(jnt);
1339  }
1340  }
1341  break;
1342  case NODE_SingleAxisHingeJoint:
1343  {
1344  dJointID jointID;
1345  struct X3D_SingleAxisHingeJoint *jnt = (struct X3D_SingleAxisHingeJoint*)joint;
1346  if(!jnt->_joint){
1347  dBodyID body1ID, body2ID;
1348  struct X3D_RigidBody *xbody1, *xbody2;
1349  xbody1 = (struct X3D_RigidBody*)jnt->body1;
1350  xbody2 = (struct X3D_RigidBody*)jnt->body2;
1351  //allow for MULL body on one side of joint, to fix to static environment
1352  body1ID = xbody1 ? xbody1->_body : NULL;
1353  body2ID = xbody2 ? xbody2->_body : NULL;
1354  jointID = dJointCreateHinge (x3dworld->_world,x3dworld->_group);
1355  dJointAttach (jointID,body1ID,body2ID);
1356  jnt->_joint = jointID;
1357  //veccopy3f(jnt->__old_anchorPoint.c,jnt->anchorPoint.c);
1358  //veccopy3f(jnt->__old_axis.c,jnt->axis.c);
1359  }
1360  if(NNC(jnt)){
1361  float axislen;
1362  if(!vecsame3f(jnt->__old_anchorPoint.c,jnt->anchorPoint.c)){
1363  dJointSetHingeAnchor (jointID,jnt->anchorPoint.c[0],jnt->anchorPoint.c[1],jnt->anchorPoint.c[2]);
1364  veccopy3f(jnt->__old_anchorPoint.c,jnt->anchorPoint.c);
1365  }
1366  axislen = veclength3f(jnt->axis.c);
1367  if(axislen < .1){
1368  //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1369  jnt->axis.c[0] = jnt->axis.c[1] = 0.0f;
1370  jnt->axis.c[2] = 1.0f;
1371  }
1372  if(!vecsame3f(jnt->__old_axis.c,jnt->axis.c)){
1373  dJointSetHingeAxis (jointID,jnt->axis.c[0],jnt->axis.c[1],jnt->axis.c[2]);
1374  veccopy3f(jnt->__old_axis.c,jnt->axis.c);
1375  }
1376  jnt->_forceout = forceout_from_names(jnt->forceOutput.n,jnt->forceOutput.p);
1377  dJointSetHingeParam (jnt->_joint,dParamLoStop,jnt->minAngle);
1378  dJointSetHingeParam (jnt->_joint,dParamHiStop,jnt->maxAngle);
1379  dJointSetHingeParam (jnt->_joint,dParamBounce,jnt->stopBounce);
1380  dJointSetHingeParam (jnt->_joint,dParamStopERP,jnt->stopErrorCorrection);
1381  MNC(jnt);
1382  }
1383  }
1384  break;
1385  case NODE_DoubleAxisHingeJoint:
1386  {
1387  //see the ODE demo_buggy for Hinge2 example of steerable wheel
1388  dJointID jointID;
1389  double avel;
1390  int method_extension_axis1Angle;
1391  struct X3D_DoubleAxisHingeJoint *jnt = (struct X3D_DoubleAxisHingeJoint*)joint;
1392 
1393  if(!jnt->_joint){
1394  dBodyID body1ID, body2ID;
1395  struct X3D_RigidBody *xbody1, *xbody2;
1396  xbody1 = (struct X3D_RigidBody*)jnt->body1;
1397  xbody2 = (struct X3D_RigidBody*)jnt->body2;
1398  //allow for MULL body on one side of joint, to fix to static environment
1399  body1ID = xbody1 ? xbody1->_body : NULL;
1400  body2ID = xbody2 ? xbody2->_body : NULL;
1401  jointID = dJointCreateHinge2 (x3dworld->_world,x3dworld->_group);
1402  //body1 should be the car, body2 the wheel
1403  dJointAttach (jointID,body1ID,body2ID);
1404  jnt->_joint = jointID;
1405  dJointSetHinge2Anchor(jnt->_joint,jnt->anchorPoint.c[0],jnt->anchorPoint.c[1],jnt->anchorPoint.c[2]);
1406 
1407  //axis 1 should be the (vertical) steering axis
1408  dJointSetHinge2Axis1(jnt->_joint,jnt->axis1.c[0],jnt->axis1.c[1],jnt->axis1.c[2]);
1409  //axis 2 should be the (horizontal) wheel axle
1410  dJointSetHinge2Axis2(jnt->_joint,jnt->axis2.c[0],jnt->axis2.c[1],jnt->axis2.c[2]);
1411 
1412  }
1413  if(NNC(jnt)){
1414  float axislen = veclength3f(jnt->axis1.c);
1415  if(axislen < .1){
1416  //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1417  jnt->axis1.c[0] = jnt->axis1.c[1] = 0.0f;
1418  jnt->axis1.c[2] = 1.0f;
1419  }
1420  if(!vecsame3f(jnt->__old_axis1.c,jnt->axis1.c)){
1421  dJointSetHinge2Axis1(jnt->_joint,jnt->axis1.c[0],jnt->axis1.c[1],jnt->axis1.c[2]);
1422  veccopy3f(jnt->__old_axis1.c,jnt->axis1.c);
1423  }
1424  if(!vecsame3f(jnt->__old_anchorPoint.c,jnt->anchorPoint.c)){
1425  dJointSetHinge2Anchor(jnt->_joint,jnt->anchorPoint.c[0],jnt->anchorPoint.c[1],jnt->anchorPoint.c[2]);
1426  veccopy3f(jnt->__old_anchorPoint.c,jnt->anchorPoint.c);
1427  }
1428  if(!vecsame3f(jnt->__old_axis2.c,jnt->axis2.c)){
1429  dJointSetHinge2Axis2(jnt->_joint,jnt->axis2.c[0],jnt->axis2.c[1],jnt->axis2.c[2]);
1430  veccopy3f(jnt->__old_axis2.c,jnt->axis2.c);
1431  }
1432  jnt->_forceout = forceout_from_names(jnt->forceOutput.n,jnt->forceOutput.p);
1433  dJointSetHinge2Param (jnt->_joint,dParamBounce1,jnt->stopBounce1);
1434  dJointSetHinge2Param (jnt->_joint,dParamStopERP1,jnt->stopErrorCorrection1);
1435  dJointSetHinge2Param (jnt->_joint,dParamSuspensionERP,jnt->suspensionErrorCorrection);
1436  dJointSetHinge2Param (jnt->_joint,dParamSuspensionCFM,jnt->suspensionForce);
1437 
1438  dJointSetHinge2Param (jnt->_joint,dParamFMax,jnt->maxTorque1);
1439  dJointSetHinge2Param (jnt->_joint,dParamLoStop,jnt->minAngle1);
1440  dJointSetHinge2Param (jnt->_joint,dParamHiStop,jnt->maxAngle1);
1441  dJointSetHinge2Param (jnt->_joint,dParamFudgeFactor,0.1);
1442 
1443  MNC(jnt);
1444  }
1445  //per-frame
1446  //motor
1447  dJointSetHinge2Param (jnt->_joint,dParamVel2,jnt->desiredAngularVelocity2);
1448  dJointSetHinge2Param (jnt->_joint,dParamFMax2,jnt->maxTorque2);
1449  //steering
1450  avel = jnt->desiredAngularVelocity1;
1451  method_extension_axis1Angle = 0;
1452  if(method_extension_axis1Angle){
1453  //the specs don't have an axis1Angle on DoubleAxisHingeJoint - too bad
1454  //because here is how easy it would be to steer a car
1455  //instead, you have to chain a motor to axis1
1456  double agap = jnt->axis1Angle - dJointGetHinge2Angle1 (jnt->_joint);
1457  //printf("agap %lf = %f - %lf\n",agap,jnt->axis1Angle,dJointGetHinge2Angle1 (jnt->_joint));
1458  double avel = agap / .1 * jnt->desiredAngularVelocity1;
1459  if (agap > 0.1) avel = jnt->desiredAngularVelocity1;
1460  if (agap < -0.1) avel = -jnt->desiredAngularVelocity1;
1461  }
1462  dJointSetHinge2Param (jnt->_joint,dParamVel,avel);
1463 
1464  }
1465  break;
1466  case NODE_SliderJoint:
1467  {
1468  dJointID jointID;
1469  struct X3D_SliderJoint *jnt = (struct X3D_SliderJoint*)joint;
1470  if(!jnt->_joint){
1471  dBodyID body1ID, body2ID;
1472  struct X3D_RigidBody *xbody1, *xbody2;
1473  xbody1 = (struct X3D_RigidBody*)jnt->body1;
1474  xbody2 = (struct X3D_RigidBody*)jnt->body2;
1475  //allow for MULL body on one side of joint, to fix to static environment
1476  body1ID = xbody1 ? xbody1->_body : NULL;
1477  body2ID = xbody2 ? xbody2->_body : NULL;
1478  jointID = dJointCreateSlider (x3dworld->_world,x3dworld->_group);
1479  dJointAttach (jointID,body1ID,body2ID);
1480  jnt->_joint = jointID;
1481  }
1482  if(NNC(jnt)){
1483  float axislen = veclength3f(jnt->axis.c);
1484  if(axislen < .1){
1485  //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1486  jnt->axis.c[0] = jnt->axis.c[1] = 0.0f;
1487  jnt->axis.c[2] = 1.0f;
1488  }
1489  if(!vecsame3f(jnt->__old_axis.c,jnt->axis.c)){
1490  dJointSetSliderAxis (jnt->_joint,jnt->axis.c[0],jnt->axis.c[1],jnt->axis.c[2]);
1491  veccopy3f(jnt->__old_axis.c,jnt->axis.c);
1492  }
1493  jnt->_forceout = forceout_from_names(jnt->forceOutput.n,jnt->forceOutput.p);
1494  dJointSetSliderParam (jnt->_joint,dParamBounce,jnt->stopBounce);
1495  dJointSetSliderParam (jnt->_joint,dParamStopERP,jnt->stopErrorCorrection);
1496 
1497  MNC(jnt);
1498  }
1499  }
1500  break;
1501  case NODE_UniversalJoint:
1502  {
1503  // http://ode.org/ode-latest-userguide.html#sec_7_3_4
1504  // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html#UniversalJoint
1505  dJointID jointID;
1506  struct X3D_UniversalJoint *jnt = (struct X3D_UniversalJoint*)joint;
1507  if(!jnt->_joint){
1508  dBodyID body1ID, body2ID;
1509  struct X3D_RigidBody *xbody1, *xbody2;
1510  xbody1 = (struct X3D_RigidBody*)jnt->body1;
1511  xbody2 = (struct X3D_RigidBody*)jnt->body2;
1512  //allow for MULL body on one side of joint, to fix to static environment
1513  body1ID = xbody1 ? xbody1->_body : NULL;
1514  body2ID = xbody2 ? xbody2->_body : NULL;
1515  jointID = dJointCreateUniversal (x3dworld->_world,x3dworld->_group);
1516  dJointAttach (jointID,body1ID,body2ID);
1517  jnt->_joint = jointID;
1518  //anchor point can be 0 0 0. If so, our vecsame3f technique below fails to set UniversalAnchor at least once.
1519  //so we do the zero-capable fields in the _joint section once
1520  dJointSetUniversalAnchor (jnt->_joint,jnt->anchorPoint.c[0],jnt->anchorPoint.c[1],jnt->anchorPoint.c[2]);
1521  }
1522  if(NNC(jnt)){
1523  float axislen;
1524  if(!vecsame3f(jnt->__old_anchorPoint.c,jnt->anchorPoint.c)){
1525  dJointSetUniversalAnchor (jnt->_joint,jnt->anchorPoint.c[0],jnt->anchorPoint.c[1],jnt->anchorPoint.c[2]);
1526  veccopy3f(jnt->__old_anchorPoint.c,jnt->anchorPoint.c);
1527  }
1528 
1529  axislen = veclength3f(jnt->axis1.c);
1530  if(axislen < .1){
1531  //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1532  jnt->axis1.c[0] = jnt->axis1.c[1] = 0.0f;
1533  jnt->axis1.c[2] = 1.0f;
1534  }
1535  axislen = veclength3f(jnt->axis2.c);
1536  if(axislen < .1){
1537  //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1538  jnt->axis2.c[0] = jnt->axis2.c[2] = 0.0f;
1539  jnt->axis2.c[1] = 1.0f;
1540  }
1541  if(!vecsame3f(jnt->__old_axis1.c,jnt->axis1.c)){
1542  dJointSetUniversalAxis1 (jnt->_joint,jnt->axis1.c[0],jnt->axis1.c[1],jnt->axis1.c[2]);
1543  veccopy3f(jnt->__old_axis1.c,jnt->axis1.c);
1544  }
1545  if(!vecsame3f(jnt->__old_axis2.c,jnt->axis2.c)){
1546  dJointSetUniversalAxis2(jnt->_joint,jnt->axis2.c[0],jnt->axis2.c[1],jnt->axis2.c[2]);
1547  veccopy3f(jnt->__old_axis2.c,jnt->axis2.c);
1548  }
1549  jnt->_forceout = forceout_from_names(jnt->forceOutput.n,jnt->forceOutput.p);
1550  dJointSetUniversalParam (jnt->_joint,dParamBounce1,jnt->stop1Bounce);
1551  dJointSetUniversalParam (jnt->_joint,dParamStopERP1,jnt->stop1ErrorCorrection);
1552  dJointSetUniversalParam (jnt->_joint,dParamBounce2,jnt->stop2Bounce);
1553  dJointSetUniversalParam (jnt->_joint,dParamStopERP2,jnt->stop2ErrorCorrection);
1554 
1555  MNC(jnt);
1556  }
1557  }
1558  break;
1559  case NODE_MotorJoint:
1560  {
1561  // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html#MotorJoint
1562  // x3dmotorjoint only ode Angular AMotor, (no Linear LMotor parameters)
1563  // only ode so-called User mode
1564  // http://ode.org/ode-latest-userguide.html#sec_7_5_1
1565  // table of joint parameters
1566  // dParamVel and dParamFMax relate specifically to AMotor joints
1567  dJointID jointID;
1568  struct X3D_MotorJoint *jnt = (struct X3D_MotorJoint*)joint;
1569  if(!jnt->_joint){
1570  dBodyID body1ID, body2ID;
1571  struct X3D_RigidBody *xbody1, *xbody2;
1572  xbody1 = (struct X3D_RigidBody*)jnt->body1;
1573  xbody2 = (struct X3D_RigidBody*)jnt->body2;
1574  //allow for MULL body on one side of joint, to fix to static environment
1575  body1ID = xbody1 ? xbody1->_body : NULL;
1576  body2ID = xbody2 ? xbody2->_body : NULL;
1577  jointID = dJointCreateAMotor (x3dworld->_world,x3dworld->_group);
1578  dJointAttach (jointID,body1ID,body2ID);
1579  jnt->_joint = jointID;
1580  dJointSetAMotorMode (jnt->_joint,dAMotorUser);
1581  //dJointSetAMotorMode (jointID,dAMotorEuler);
1582  }
1583  if(NNC(jnt)){
1584  float axislen;
1585  dJointSetAMotorNumAxes (jnt->_joint,jnt->enabledAxes);
1586  //rel: relative to: 0-static 1-first body 2-2nd body
1587  if(jnt->enabledAxes >0 ){
1588  axislen = veclength3f(jnt->motor1Axis.c);
1589  if(axislen < .1){
1590  //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1591  jnt->motor1Axis.c[0] = jnt->motor1Axis.c[1] = 0.0f;
1592  jnt->motor1Axis.c[2] = 1.0f;
1593  }
1594 
1595  if(!vecsame3f(jnt->__old_motor1Axis.c,jnt->motor1Axis.c)){
1596  dJointSetAMotorAxis (jnt->_joint,0,0, jnt->motor1Axis.c[0],jnt->motor1Axis.c[1],jnt->motor1Axis.c[2]);
1597  veccopy3f(jnt->__old_motor1Axis.c,jnt->motor1Axis.c);
1598  }
1599  if(jnt->__old_axis1Angle != jnt->axis1Angle){
1600  dJointSetAMotorAngle (jnt->_joint, 0, jnt->axis1Angle);
1601  jnt->__old_axis1Angle = jnt->axis1Angle;
1602  }
1603  }
1604  if(jnt->enabledAxes >1 ){
1605  axislen = veclength3f(jnt->motor2Axis.c);
1606  if(axislen < .1){
1607  //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1608  jnt->motor2Axis.c[0] = jnt->motor2Axis.c[1] = 0.0f;
1609  jnt->motor2Axis.c[2] = 1.0f;
1610  }
1611 
1612  if(!vecsame3f(jnt->__old_motor2Axis.c,jnt->motor2Axis.c)){
1613  dJointSetAMotorAxis (jnt->_joint,1,1, jnt->motor2Axis.c[0],jnt->motor2Axis.c[1],jnt->motor2Axis.c[2]);
1614  veccopy3f(jnt->__old_motor2Axis.c,jnt->motor2Axis.c);
1615  }
1616  if(jnt->__old_axis2Angle != jnt->axis2Angle){
1617  dJointSetAMotorAngle (jnt->_joint, 1, jnt->axis2Angle);
1618  jnt->__old_axis2Angle = jnt->axis2Angle;
1619  }
1620  //dJointSetAMotorParam(jointID,dParamFMax2,jnt->axis2Torque);
1621  }
1622  if(jnt->enabledAxes >2 ){
1623  axislen = veclength3f(jnt->motor3Axis.c);
1624  if(axislen < .1){
1625  //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1626  jnt->motor3Axis.c[0] = jnt->motor3Axis.c[1] = 0.0f;
1627  jnt->motor3Axis.c[2] = 1.0f;
1628  }
1629 
1630  if(!vecsame3f(jnt->__old_motor3Axis.c,jnt->motor3Axis.c)){
1631  dJointSetAMotorAxis (jnt->_joint,2,2, jnt->motor3Axis.c[0],jnt->motor3Axis.c[1],jnt->motor3Axis.c[2]);
1632  veccopy3f(jnt->__old_motor3Axis.c,jnt->motor3Axis.c);
1633  }
1634  if(jnt->__old_axis3Angle != jnt->axis3Angle){
1635  dJointSetAMotorAngle (jnt->_joint, 2, jnt->axis3Angle);
1636  jnt->__old_axis3Angle = jnt->axis3Angle;
1637  }
1638  //dJointSetAMotorParam(jointID,dParamFMax3,jnt->axis3Torque);
1639  }
1640  jnt->_forceout = forceout_from_names(jnt->forceOutput.n,jnt->forceOutput.p);
1641 
1642  dJointSetAMotorParam (jnt->_joint,dParamBounce1,jnt->stop1Bounce);
1643  dJointSetAMotorParam (jnt->_joint,dParamStopERP1,jnt->stop1ErrorCorrection);
1644  dJointSetAMotorParam (jnt->_joint,dParamBounce2,jnt->stop2Bounce);
1645  dJointSetAMotorParam (jnt->_joint,dParamStopERP2,jnt->stop2ErrorCorrection);
1646  dJointSetAMotorParam (jnt->_joint,dParamBounce3,jnt->stop3Bounce);
1647  dJointSetAMotorParam (jnt->_joint,dParamStopERP3,jnt->stop3ErrorCorrection);
1648 
1649  //addMotorTorques is a macro function in ODE that finds the bodies involve and
1650  //applies the torques to the bodies
1651  //if(jnt->autoCalc == TRUE)
1652  dJointAddAMotorTorques(jnt->_joint, jnt->axis1Torque, jnt->axis2Torque, jnt->axis3Torque);
1653  MNC(jnt);
1654 
1655  }
1656  //per-frame torque - this will cause an acceleration, don't know if that's
1657  //what the x3dmotorjoint torque fields were meant for
1658  //dJointAddAMotorTorques(jnt->_joint, jnt->axis1Torque, jnt->axis2Torque, jnt->axis3Torque);
1659  if(jnt->autoCalc == FALSE){
1660  //user sets angles on each frame
1661  if(jnt->enabledAxes >0)
1662  dJointSetAMotorAngle (jnt->_joint, 0, jnt->axis1Angle);
1663  if(jnt->enabledAxes >1)
1664  dJointSetAMotorAngle (jnt->_joint, 1, jnt->axis2Angle);
1665  if(jnt->enabledAxes >2)
1666  dJointSetAMotorAngle (jnt->_joint, 2, jnt->axis3Angle);
1667  }
1668  }
1669  break;
1670  default:
1671  break;
1672  } //switch (joint type)
1673  }
1674 
1675  //RUN PHYSICS ENGINE
1676  dSpaceCollide (space,0,&nearCallback);
1677  if (!rbp_pause) {
1678  if(x3dworld->preferAccuracy){
1679  //PUNISHING GIANT MATRIX PUSHED ONTO C STACK FOR GIANT MATRIX SOLUTION
1680  double step_fraction = STEP_SIZE;
1681  dWorldStep (x3dworld->_world,step_fraction);
1682  }else{
1683  //SO CALLED QUICK-STEP METHOD WHICH IS FASTER, THE NORMAL WAY TO DO IT
1684  //dWorldSetQuickStepNumIterations (x3dworld->_world, x3dworld->iterations);
1685  double step_fraction = STEP_SIZE / (double)x3dworld->iterations;
1686  for(nstep=0;nstep<x3dworld->iterations;nstep++)
1687  dWorldQuickStep (x3dworld->_world,step_fraction);
1688  }
1689  }
1690 
1691  //do eventOuts
1692  //Rigidbody -> Collidable
1693  for(i=0;i<x3dworld->bodies.n;i++){
1694  x3dbody = (struct X3D_RigidBody*)x3dworld->bodies.p[i];
1695  if(x3dbody->_body){
1696  //if not fixed, it will have a body that maybe moved
1697  //ATTEMPT 5 and 6
1698  //we set and mark both x3dbody and top-level collidable translation,rotation
1699  //top level collidable: we concatonate x3dboy * top-collidable transform
1700  const dReal *dpos, *dquat;
1701  //dReal *drot;
1702  Quaternion quat;
1703  double xyza[4];
1704 
1705  dpos = dBodyGetPosition (x3dbody->_body);
1706  //printf("dpos = %lf %lf %lf\n",dpos[0],dpos[1],dpos[2]);
1707  dquat = dBodyGetQuaternion(x3dbody->_body);
1708  quat.x = dquat[1], quat.y = dquat[2], quat.z = dquat[3], quat.w = dquat[0];
1709  quaternion_to_vrmlrot(&quat,&xyza[0],&xyza[1],&xyza[2],&xyza[3]);
1710  //double2float(x3dbody->position.c,dpos,3);
1711  x3dbody->position.c[0] = dpos[0];x3dbody->position.c[1] = dpos[1];x3dbody->position.c[2] = dpos[2];
1712  double2float(x3dbody->orientation.c,xyza,4);
1713  MARK_EVENT(X3D_NODE(x3doffset),offsetof(struct X3D_RigidBody,position));
1714  MARK_EVENT(X3D_NODE(x3doffset),offsetof(struct X3D_RigidBody,orientation));
1715 
1716  for(k=0;k<x3dbody->geometry.n;k++){
1717  float *translation, *rotation;
1718  struct X3D_CollidableOffset* x3doffset = (struct X3D_CollidableOffset*)x3dbody->geometry.p[k];
1719  translation = x3doffset->translation.c;
1720  rotation = x3doffset->rotation.c;
1721  //ATTEMPT 5: concatonate rigidbody transform with top-level collidable transform
1722  if(x3doffset->_nodeType == NODE_CollidableOffset){
1723  //body_T * body_R * geom_T * geom_R ==
1724  //(body_T + body_R*geom_T) * body_R*geom R
1725  double geomT[3], geomR[4];
1726  Quaternion geomQ;
1727  float2double(geomT,x3doffset->_initialTranslation.c,3);
1728  float2double(geomR,x3doffset->_initialRotation.c,4);
1729  vrmlrot_to_quaternion(&geomQ,geomR[0],geomR[1],geomR[2],geomR[3]);
1730  quaternion_rotationd(geomT,&quat,geomT);
1731  double2float(translation,geomT,3);
1732  vecadd3f(translation,translation,x3dbody->position.c);
1733  quaternion_multiply(&geomQ,&quat,&geomQ);
1734  quaternion_to_vrmlrot(&geomQ,&geomR[0],&geomR[1],&geomR[2],&geomR[3]);
1735  double2float(rotation,geomR,4);
1736  }
1737  if(x3doffset->_nodeType == NODE_CollidableShape){
1738  veccopy3f(translation,x3dbody->position.c);
1739  veccopy4f(rotation,x3dbody->orientation.c);
1740  }
1741  //double2float(translation,dpos,3);
1742  //double2float(rotation,xyza,4);
1743  if(0) if(i==0){
1744  //some debug
1745  static int loopcount = 0;
1746  if(loopcount < 30){
1747  printf("pos %lf %lf %lf\n",dpos[0],dpos[1],dpos[2]);
1748  printf("rot %f %f %f %f\n",rotation[0],rotation[1],rotation[2],rotation[3]);
1749  printf("trn %f %f %f\n",translation[0],translation[1],translation[2]);
1750  loopcount++;
1751  }
1752  }
1753  //if(1) printf("transout %f %f %f\n",translation[0],translation[1],translation[2]);
1754  MARK_EVENT(X3D_NODE(x3doffset),offsetof(struct X3D_CollidableOffset,translation));
1755  MARK_EVENT(X3D_NODE(x3doffset),offsetof(struct X3D_CollidableOffset,rotation));
1756 
1757  x3doffset->_change++;
1758  }
1759  }
1760  } //for bodies
1761  for(i=0;i<x3dworld->joints.n;i++){
1762  struct X3D_Node *joint = (struct X3D_Node*)x3dworld->joints.p[i];
1763  // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html#X3DRigidJointNode
1764  // forceOutput = [NONE] ["NONE","ALL", specific output field name ie for MotorJoint "motor1AngleRate"
1765  switch(joint->_nodeType){
1766  case NODE_BallJoint:
1767  {
1768  //dJointID jointID;
1769  struct X3D_BallJoint *jnt = (struct X3D_BallJoint*)joint;
1770  if(jnt->_forceout){
1771  if(jnt->_forceout & FORCEOUT_body1AnchorPoint){
1772  // jnt->body1AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1773  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_BallJoint,body1AnchorPoint));
1774  }
1775  if(jnt->_forceout & FORCEOUT_body2AnchorPoint){
1776  // jnt->body2AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1777  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_BallJoint,body2AnchorPoint));
1778  }
1779 
1780  }
1781  }
1782  break;
1783  case NODE_SingleAxisHingeJoint:
1784  {
1785  //dJointID jointID;
1786  struct X3D_SingleAxisHingeJoint *jnt = (struct X3D_SingleAxisHingeJoint*)joint;
1787  if(jnt->_forceout){
1788 
1789  if(jnt->_forceout & FORCEOUT_angle){
1790  // jnt->angle = dJointGetAMotorAngle( jnt->_joint, 0 );
1791  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SingleAxisHingeJoint,angle));
1792  }
1793  if(jnt->_forceout & FORCEOUT_angleRate){
1794  // jnt->angleRate = dJointGetAMotorAngle( jnt->_joint, 0 );
1795  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SingleAxisHingeJoint,angleRate));
1796  }
1797 
1798  if(jnt->_forceout & FORCEOUT_body1AnchorPoint){
1799  // jnt->body1AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1800  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SingleAxisHingeJoint,body1AnchorPoint));
1801  }
1802  if(jnt->_forceout & FORCEOUT_body2AnchorPoint){
1803  // jnt->body2AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1804  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SingleAxisHingeJoint,body2AnchorPoint));
1805  }
1806  }
1807  }
1808  break;
1809  case NODE_DoubleAxisHingeJoint:
1810  {
1811  //dJointID jointID;
1812  struct X3D_DoubleAxisHingeJoint *jnt = (struct X3D_DoubleAxisHingeJoint*)joint;
1813  if(jnt->_forceout){
1814  if(jnt->_forceout & FORCEOUT_body1AnchorPoint){
1815  // jnt->body1AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1816  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,body1AnchorPoint));
1817  }
1818  if(jnt->_forceout & FORCEOUT_body1Axis){
1819  // jnt->body1Axis = dJointGetAMotorAngle( jnt->_joint, 0 );
1820  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,body1Axis));
1821  }
1822  if(jnt->_forceout & FORCEOUT_body2AnchorPoint){
1823  // jnt->body2AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1824  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,body2AnchorPoint));
1825  }
1826  if(jnt->_forceout & FORCEOUT_body2Axis){
1827  // jnt->body2Axis = dJointGetAMotorAngle( jnt->_joint, 0 );
1828  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,body2Axis));
1829  }
1830  if(jnt->_forceout & FORCEOUT_hinge1Angle){
1831  // jnt->hinge1Angle = dJointGetAMotorAngle( jnt->_joint, 0 );
1832  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,hinge1Angle));
1833  }
1834  if(jnt->_forceout & FORCEOUT_hinge1AngleRate){
1835  // jnt->hinge1AngleRate = dJointGetAMotorAngle( jnt->_joint, 0 );
1836  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,hinge1AngleRate));
1837  }
1838  if(jnt->_forceout & FORCEOUT_hinge2Angle){
1839  // jnt->hinge2Angle = dJointGetAMotorAngle( jnt->_joint, 0 );
1840  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,hinge2Angle));
1841  }
1842  if(jnt->_forceout & FORCEOUT_hinge2AngleRate){
1843  // jnt->hinge2AngleRate = dJointGetAMotorAngle( jnt->_joint, 0 );
1844  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,hinge2AngleRate));
1845  }
1846 
1847  }
1848  }
1849  break;
1850  case NODE_SliderJoint:
1851  {
1852  //dJointID jointID;
1853  struct X3D_SliderJoint *jnt = (struct X3D_SliderJoint*)joint;
1854  if(jnt->_forceout){
1855  if(jnt->_forceout & FORCEOUT_separation){
1856  // jnt->separation = dJointGetAMotorAngle( jnt->_joint, 0 );
1857  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SliderJoint,separation));
1858  }
1859  if(jnt->_forceout & FORCEOUT_separationRate){
1860  // jnt->separationRate = dJointGetAMotorAngle( jnt->_joint, 0 );
1861  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SliderJoint,separationRate));
1862  }
1863  }
1864  }
1865  break;
1866  case NODE_UniversalJoint:
1867  {
1868  //dJointID jointID;
1869  struct X3D_UniversalJoint *jnt = (struct X3D_UniversalJoint*)joint;
1870  if(jnt->_forceout){
1871  if(jnt->_forceout & FORCEOUT_body1AnchorPoint){
1872  // jnt->body1AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1873  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_UniversalJoint,body1AnchorPoint));
1874  }
1875  if(jnt->_forceout & FORCEOUT_body1Axis){
1876  // jnt->body1Axis = dJointGetAMotorAngle( jnt->_joint, 0 );
1877  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_UniversalJoint,body1Axis));
1878  }
1879  if(jnt->_forceout & FORCEOUT_body2AnchorPoint){
1880  // jnt->body2AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1881  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_UniversalJoint,body2AnchorPoint));
1882  }
1883  if(jnt->_forceout & FORCEOUT_body2Axis){
1884  // jnt->body2Axis = dJointGetAMotorAngle( jnt->_joint, 0 );
1885  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_UniversalJoint,body2Axis));
1886  }
1887  }
1888  }
1889  break;
1890  case NODE_MotorJoint:
1891  {
1892  //dJointID jointID;
1893  struct X3D_MotorJoint *jnt = (struct X3D_MotorJoint*)joint;
1894  if(jnt->_forceout){
1895  if(jnt->_forceout & FORCEOUT_motor1Angle){
1896  jnt->motor1Angle = (float) dJointGetAMotorAngle( jnt->_joint, 0 );
1897  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor1Angle));
1898  }
1899  if(jnt->_forceout & FORCEOUT_motor1AngleRate){
1900  jnt->motor1AngleRate = (float) dJointGetAMotorAngleRate( jnt->_joint, 0 );
1901  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor1AngleRate));
1902  }
1903  if(jnt->_forceout & FORCEOUT_motor2Angle){
1904  jnt->motor2Angle = (float) dJointGetAMotorAngle( jnt->_joint, 1 );
1905  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor2Angle));
1906  }
1907  if(jnt->_forceout & FORCEOUT_motor2AngleRate){
1908  jnt->motor2AngleRate = (float) dJointGetAMotorAngleRate( jnt->_joint, 1 );
1909  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor2AngleRate));
1910  }
1911  if(jnt->_forceout & FORCEOUT_motor3Angle){
1912  jnt->motor3Angle = (float) dJointGetAMotorAngle( jnt->_joint, 2 );
1913  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor3Angle));
1914  }
1915  if(jnt->_forceout & FORCEOUT_motor3AngleRate){
1916  jnt->motor3AngleRate = (float) dJointGetAMotorAngleRate( jnt->_joint, 2 );
1917  MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor3AngleRate));
1918  }
1919  }
1920 
1921  }
1922  break;
1923  default:
1924  break;
1925  } //switch on joint
1926  } //for joints
1927  // remove all contact joints
1928  dJointGroupEmpty (contactgroup);
1929  }
1930  }
1931 
1932 }
1933 
1934 void register_CollisionCollection(struct X3D_Node * _node){
1935  if(_node->_nodeType == NODE_CollisionCollection){
1936  ppComponent_RigidBodyPhysics p;
1937  struct X3D_CollisionSensor *node = (struct X3D_CollisionSensor*)_node;
1938  p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
1939  if(!x3dcollisioncollections) x3dcollisioncollections = newVector(struct X3D_Node*,5);
1940  if(!space) dHashSpaceCreate(0); //default space
1941  vector_pushBack(struct X3D_Node*,x3dcollisioncollections,_node);
1942  MARK_NODE_COMPILED;
1943  }
1944 }
1945 void register_CollisionSensor(struct X3D_Node *_node){
1946  if(_node->_nodeType == NODE_CollisionSensor){
1947  ppComponent_RigidBodyPhysics p;
1948  struct X3D_CollisionSensor *node = (struct X3D_CollisionSensor*)_node;
1949  p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
1950  if(!x3dcollisionsensors) x3dcollisionsensors = newVector(struct X3D_Node*,5);
1951  vector_pushBack(struct X3D_Node*,x3dcollisionsensors,_node);
1952  MARK_NODE_COMPILED;
1953  }
1954 }
1955 
1956 void register_RigidBodyCollection(struct X3D_Node *_node){
1957  if(_node->_nodeType == NODE_RigidBodyCollection){
1958  ppComponent_RigidBodyPhysics p;
1959  dJointGroupID groupID;
1960  struct X3D_RigidBodyCollection *node = (struct X3D_RigidBodyCollection*)_node;
1961  p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
1962 
1963  if(!space)
1964  space = dHashSpaceCreate (0); //default space
1965  if(!world){
1966  world = dWorldCreate();
1967  dWorldSetGravity (world,node->gravity.c[0],node->gravity.c[1],node->gravity.c[2]);
1968  dWorldSetCFM (world,1e-5);
1969  dWorldSetAutoDisableFlag (world,1);
1970  //this contactgroup is a default group for doing just collisions, cleared every frame:
1971  // 1)collide (callback to make temporary joints out of contact points)
1972  // 2)physics step (uses all joints)
1973  // 3)clear contactgroup (clean out all the temporary collision joints)
1974  contactgroup = dJointGroupCreate (0);
1975 
1976  #if 1
1977 
1978  dWorldSetAutoDisableAverageSamplesCount( world, 10 );
1979 
1980  #endif
1981 
1982  dWorldSetLinearDamping(world, 0.00001);
1983  dWorldSetAngularDamping(world, 0.005);
1984  dWorldSetMaxAngularSpeed(world, 200);
1985 
1986  dWorldSetContactMaxCorrectingVel (world,0.1);
1987  dWorldSetContactSurfaceLayer (world,0.001);
1988  }
1990  //node->_space = (void*)space;
1991  groupID = dJointGroupCreate (0); //this is a contact group for joints
1992  node->_group = groupID;
1993  if(!x3dworlds) x3dworlds = newVector(struct X3D_Node*,5);
1994  vector_pushBack(struct X3D_Node*,x3dworlds,_node);
1995  MARK_NODE_COMPILED;
1996  }
1997 }
1998 
1999 // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html#TransformationHierarchy
2000 // "Nodes .. are not part of the transform hierarchy.."
2001 // 37.4.3 CollidableShape: "When placed under a part of the transformation hierarchy,
2002 // it can be used to visually represent the movement of the object."
2003 // Interpretation:
2004 // if/when put outside of the X3DRigidBodyCollection -via DEF / USE- then
2005 // the collidableshape (and any wrapper collidableOffset) transforms are pushed onto the
2006 // render_hier transform stack and the (collision/physics proxy) Shape is rendered
2007 // (versus if hidden with Switch -1, or only DEFed inside X3DRigidBodyCollection, Shape is never
2008 // rendered, but CollidableShape and CollidableOffset should output translation_changed,
2009 // and rotation_changed events, so can route to transforms containing non-proxy shapes
2010 
2011 // option: just expose compile and render virt_ functions for collidableshape and collidableoffset
2012 //- and handle prep/fin/child privately
2013 
2014 
2015 void prep_CollidableShape(struct X3D_Node *_node){
2016  struct X3D_CollidableShape *node = (struct X3D_CollidableShape*)_node;
2017  if(node){
2018  COMPILE_IF_REQUIRED
2019 
2020  //CollidableOffset will come in here too, so in perl, keep the first fields in same order
2021  //so we can caste to one, and the fields jive
2022  if(!renderstate()->render_vp) {
2023 
2024  /* rendering the viewpoint means doing the inverse transformations in reverse order (while poping stack),
2025  * so we do nothing here in that case -ncoder */
2026 
2027  /* printf ("prep_Transform, render_hier vp %d geom %d light %d sens %d blend %d prox %d col %d\n",
2028  render_vp,render_geom,render_light,render_sensitive,render_blend,render_proximity,render_collision); */
2029 
2030  /* do we have any geometry visible, and are we doing anything with geometry? */
2031  OCCLUSIONTEST
2032 
2033  if(!renderstate()->render_vp) {
2034  /* do we actually have any thing to rotate/translate/scale?? */
2035 
2036  FW_GL_PUSH_MATRIX();
2037 
2038  /* TRANSLATION */
2039  if (node->__do_trans)
2040  FW_GL_TRANSLATE_F(node->translation.c[0],node->translation.c[1],node->translation.c[2]);
2041 
2042  /* ROTATION */
2043  if (node->__do_rotation) {
2044  FW_GL_ROTATE_RADIANS(node->rotation.c[3], node->rotation.c[0],node->rotation.c[1],node->rotation.c[2]);
2045  }
2046  RECORD_DISTANCE
2047  }
2048  }
2049  }
2050 }
2051 void compile_CollidableShape(struct X3D_Node *_node){
2052  if(_node->_nodeType == NODE_CollidableShape || _node->_nodeType == NODE_CollidableOffset){
2053  ppComponent_RigidBodyPhysics p;
2054  struct X3D_CollidableShape *node = (struct X3D_CollidableShape*)_node;
2055  p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
2056 
2057  INITIALIZE_EXTENT;
2058  node->__do_trans = verify_translate ((GLfloat *)node->translation.c);
2059  node->__do_rotation = verify_rotate ((GLfloat *)node->rotation.c);
2060  MARK_NODE_COMPILED;
2061  }
2062 }
2063 
2064 void fin_CollidableShape(struct X3D_Node *_node){
2065  if(_node){
2066  if(!renderstate()->render_vp) {
2067  FW_GL_POP_MATRIX();
2068  } else {
2069  /*Rendering the viewpoint only means finding it, and calculating the reverse WorldView matrix.*/
2070  if((_node->_renderFlags & VF_Viewpoint) == VF_Viewpoint) {
2071  struct X3D_CollidableShape *node = (struct X3D_CollidableShape*)_node;
2072 
2073  FW_GL_ROTATE_RADIANS(-(((node->rotation).c[3])),((node->rotation).c[0]),((node->rotation).c[1]),((node->rotation).c[2])
2074  );
2075  FW_GL_TRANSLATE_F(-(((node->translation).c[0])),-(((node->translation).c[1])),-(((node->translation).c[2]))
2076  );
2077  }
2078  }
2079  }
2080 }
2081 void compile_CollidableOffset(struct X3D_Node *node){
2082  compile_CollidableShape(node);
2083 }
2084 void child_CollidableShape(struct X3D_Node *_node){
2085  if(_node->_nodeType == NODE_CollidableShape){
2086  ppComponent_RigidBodyPhysics p;
2087  struct X3D_CollidableShape *node = (struct X3D_CollidableShape*)_node;
2088  p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
2089  if(node->shape) render_node(node->shape);
2090  }
2091 }
2092 void prep_CollidableOffset(struct X3D_Node *node){
2093  prep_CollidableShape(node);
2094 }
2095 void fin_CollidableOffset(struct X3D_Node *node){
2096  fin_CollidableShape(node);
2097 }
2098 void child_CollidableOffset(struct X3D_Node *_node){
2099  if(_node->_nodeType == NODE_CollidableOffset){
2100  ppComponent_RigidBodyPhysics p;
2101  struct X3D_CollidableOffset *node = (struct X3D_CollidableOffset*)_node;
2102  p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
2103 
2104  //->collidable is an SFNode. Options:
2105  // a) we could go through normalChildren(n=1,p=&collidable) which would call prep and fin and child for us
2106  // b) we can call prep_, child_ and fin_ ourselves
2107  if(node->collidable) {
2108  switch(node->collidable->_nodeType){
2109  case NODE_CollidableOffset:
2110  prep_CollidableOffset(node->collidable);
2111  child_CollidableOffset(node->collidable);
2112  fin_CollidableOffset(node->collidable);
2113  break;
2114  case NODE_CollidableShape:
2115  prep_CollidableShape(node->collidable);
2116  child_CollidableShape(node->collidable);
2117  fin_CollidableShape(node->collidable);
2118  break;
2119  default:
2120  break;
2121  }
2122  }
2123  }
2124 }
2125 
2126 void do_CollisionSensorTick0(struct X3D_CollisionSensor *node){
2127  //if we call this from the routing do_...Tick
2128  //that happens before rbp_run_phsyics() is called
2129  //so in rbp_run_physics the first thing we can do is clear the contacts in any sensors
2130  //then any contacts generated during physics can come out of here in the next routing session
2131  if(!static_did_physics_since_last_Tick) return;
2132  static_did_physics_since_last_Tick = 0;
2133  if(!node->enabled) return;
2134 
2135  if(node->contacts.n){
2136  if(node->isActive == FALSE){
2137  node->isActive = TRUE;
2138  MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,isActive));
2139  }
2140  MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,contacts));
2141  //node->contacts.n = 0;
2142  // leave at 100 FREE_IF_NZ(node->contacts.p);
2143  }else{
2144  if(node->isActive == TRUE){
2145  node->isActive = FALSE;
2146  MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,isActive));
2147  //do I need to route n=0?
2148  //leave at 100 node->contacts.p = NULL; //if I don't set to null and mark, then CRoutes bombs in some cleanup code.
2149  //MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,contacts));
2150  }
2151  }
2152  if(node->isActive){
2153  if(node->intersections.n){
2154  MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,intersections));
2155  //node->intersections.n = 0;
2156  //leave at 100 FREE_IF_NZ(node->intersections.p);
2157  }
2158  //else{
2159  // MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,intersections));
2160  //}
2161  }
2162 }
2163 void do_CollisionSensorTick(void * ptr){
2164  if(ptr)
2165  do_CollisionSensorTick0((struct X3D_CollisionSensor *)ptr);
2166 }
2167 
2168 void add_physics(struct X3D_Node *node){
2169  switch(node->_nodeType){
2170  case NODE_CollisionSensor:
2171  //Nov. 2016: H: this should be in do_first ie do_CollisionSensor
2172  //which gets called before routing ie so if there's a Contact it can be routed
2173  //its almost the same place as physics, which is just after do_events,routing
2174  // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/concepts.html#ExecutionModel
2175  register_CollisionSensor(node);
2176  break;
2177  case NODE_CollisionCollection:
2178  //CollisionCollections are x3dchild nodes, and can appear naked in scenegraph
2179  //or in CollisionSensor field, or in RigidBodyCollection field
2180  //ie might be DEF/USED
2181  register_CollisionCollection(node);
2182  break;
2183  case NODE_RigidBodyCollection:
2184  //OK good.
2185  register_RigidBodyCollection(node);
2186  break;
2187  //Q. what about CollisionSpace, CollisionCollection - should they be registered here?
2188  default:
2189  break;
2190  }
2191 }
2192 
2193 //END MIT LIC <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
2194 
2195 #else //else no ode phyiscs engine, just stubs
2196 
2197 void compile_CollidableShape(struct X3D_Node *node){
2198 }
2199 void prep_CollidableShape(struct X3D_Node *node){
2200 }
2201 void fin_CollidableShape(struct X3D_Node *node){
2202 }
2203 void child_CollidableShape(struct X3D_Node *node){
2204 }
2205 void compile_CollidableOffset(struct X3D_Node *node){
2206 }
2207 void prep_CollidableOffset(struct X3D_Node *node){
2208 }
2209 void fin_CollidableOffset(struct X3D_Node *node){
2210 }
2211 void child_CollidableOffset(struct X3D_Node *node){
2212 }
2213 void rbp_run_physics(){
2214 }
2215 void add_physics(struct X3D_Node *node){
2216 }
2217 void do_CollisionSensorTick(void * ptr){
2218 }
2219 
2220 #endif
2221 
2222 
2223 
2224 
X3D_CollisionSensor
Definition: Structs.h:3398
X3D_CollidableOffset
Definition: Structs.h:3272
X3D_Node
Definition: Structs.h:2502
X3D_SliderJoint
Definition: Structs.h:9245
X3D_Cylinder
Definition: Structs.h:3995
X3D_Shape
Definition: Structs.h:9107
X3D_SingleAxisHingeJoint
Definition: Structs.h:9207
X3D_Sphere
Definition: Structs.h:9312
X3D_UniversalJoint
Definition: Structs.h:10376
X3D_RigidBodyCollection
Definition: Structs.h:8799
X3D_CollisionCollection
Definition: Structs.h:3366
Vector
Definition: Vector.h:36
pComponent_RigidBodyPhysics
Definition: Component_RigidBodyPhysics.c:64
X3D_Coordinate
Definition: Structs.h:3837
X3D_Box
Definition: Structs.h:3041
X3D_TriangleSet
Definition: Structs.h:10258
X3D_Contact
Definition: Structs.h:3757
X3D_DoubleAxisHingeJoint
Definition: Structs.h:4170
X3D_CollidableShape
Definition: Structs.h:3304
X3D_BallJoint
Definition: Structs.h:2818
X3D_CollisionSpace
Definition: Structs.h:3422
X3D_MotorJoint
Definition: Structs.h:7177
X3D_RigidBody
Definition: Structs.h:8749
Uni_String
Definition: Structs.h:51