FreeWRL / FreeX3D 4.3.0
Component_RigidBodyPhysics.c
1/*
2
3
4X3D 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
65 int something;
67void *Component_RigidBodyPhysics_constructor(){
68 void *v = MALLOCV(sizeof(struct pComponent_RigidBodyPhysics));
69 memset(v,0,sizeof(struct pComponent_RigidBodyPhysics));
70 return v;
71}
72void Component_RigidBodyPhysics_init(struct tComponent_RigidBodyPhysics *t){
73 //public
74 //private
75 t->prv = Component_RigidBodyPhysics_constructor();
76 {
78 p->something = 0;
79 }
80}
81void 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:
89int NNC0(struct X3D_Node* node){
90 //NNC Node Needs Compiling
91 return NODE_NEEDS_COMPILING;
92 //return FALSE;
93}
94void MNC0(struct X3D_Node* node){
95 //MNC Mark Node Compiled
96 MARK_NODE_COMPILED;
97}
98void 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
108void rbp_run_physics();
109void set_physics();
110
111//#undef WITH_RBP
112#ifdef WITH_RBP
113//START MIT LIC >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
114
115/*
116Physics options considered:
1170. PAL - physics abstraction layer (in c++, not maintained)
1181. bullet - very popular, c++/no flat-c interface, large
1192. ode - c api (even though implemented in c++),
120 - maps well to x3d, specs say 'implemented by ode' in one place
121decision feb 2016: ode
122
123X3D
124http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html
125
126ODE
127http://ode-wiki.org/wiki/index.php?title=Manual:_Rigid_Body_Functions
128- body, world
129http://ode-wiki.org/wiki/index.php?title=Manual:_Collision_Detection
130- geoms, geom postion, geom class, collide(), spaces, contact points
131
132X3D ODE
133CollidableShape geoms
134CollidableOffset GeomTransform T ?
135 - both: GeomSetPosition,Rotation,Quat,
136CollisionCollection Geom Class / category bits
137CollisionSensor dSpaceCollide() collide2() callback()
138CollisionSpace spaces
139Contact Contact points
140RigidBody body
141RigidBodyCollection world
142
143Initializing physics nodes:
144Freewrl has a few ways of initializing a node:
145A. 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
147B. 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
149C. in startofloopnodeupdates() done once per frame
150D. after event processing in execution model ie our rbp_run_physics()
151
152Of the physics nodes, only CollidableShape and its extra-transform wrapper CollidableOffset need
153to be visited on a render_hier pass for visual rendering.
154
155X3D declared Node Init Field value node types
156RigidBodyCollection A,D RigidBody
157RigidBody D CollidableShape, CollidableOffset
158CollisionCollection D CollidableShape, CollidableOffset, CollisionSpace
159CollisionSpace D CollidableShape, CollidableOffset, CollisionSpace
160CollidableShape B,D Shape
161CollidableOffset B,D CollidableShape
162CollisionSensor A,D CollisionCollection
163
164Generated Node Generating Node
165CollisionCollection RigidBodyCollection
166Contact 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
184typedef struct MyObject {
185 dBodyID body; // the body
186 dGeomID geom[GPB]; // geometries representing this body
187} MyObject;
188
189static int num=0; // number of objects in simulation
190static int nextobj=0; // next object to recycle if num==NUM
191static dWorldID world = NULL;
192static dSpaceID space = NULL;;
193static MyObject obj[NUM];
194static dJointGroupID contactgroup; //used by nearCallback for per-frame flushable contact joints
195static int selected = -1; // selected object
196static int show_aabb = 0; // show geom AABBs?
197static int show_contacts = 0; // show contact points?
198static int random_pos = 1; // drop objects from random position?
199static int write_world = 0;
200static int show_body = 0;
201
202typedef struct MyFeedback {
203 dJointFeedback fb;
204 bool first;
205}MyFeedback;
206static int doFeedback=0;
207static MyFeedback feedbacks[MAX_FEEDBACKNUM];
208static int fbnum=0;
209
210
211//our registered lists (should be in gglobal p-> or scene->)
212static struct Vector *x3dworlds = NULL;
213static struct Vector *x3dcollisionsensors = NULL;
214static 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
219struct X3D_CollidableShape * getCollidableShapeFromData(void *cdata){
220 struct X3D_CollidableShape * xshape = NULL;
221 if(cdata)
222 xshape = (struct X3D_CollidableShape*)cdata;
223 return xshape;
224}
225struct 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}
245struct 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}
266static struct X3D_Contact static_contacts_p[100];
267static int static_contacts_n = 0;
268static struct X3D_Contact *static_contacts_initializer = NULL;
269static 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
274static 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 %p\n",xsens1);
320 if(xsens2) printf("have csens2 %p\n",xsens2);
321 if(xcol1) printf("have ccol1 %p\n",xcol1);
322 if(xcol2) printf("have ccol2 %p\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
482static int init_rbp_once = 0;
483//static dThreadingImplementationID threading;
484//static dThreadingThreadPoolID pool;
485int 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}
505void 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}
522enum {
523FORCEOUT_ALL = 0xFFFFFFFF,
524FORCEOUT_NONE = 0x00000000,
525//MOTOR
526FORCEOUT_motor1Angle = 1 << 0,
527FORCEOUT_motor1AngleRate= 1 << 1,
528FORCEOUT_motor2Angle = 1 << 2,
529FORCEOUT_motor2AngleRate= 1 << 3,
530FORCEOUT_motor3Angle = 1 << 4,
531FORCEOUT_motor3AngleRate= 1 << 5,
532//DOUBLEAXISHING
533FORCEOUT_body1AnchorPoint=1 << 6,
534FORCEOUT_body1Axis = 1 << 7,
535FORCEOUT_body2AnchorPoint=1 << 8,
536FORCEOUT_body2Axis = 1 << 9,
537FORCEOUT_hinge1Angle = 1 << 10,
538FORCEOUT_hinge1AngleRate= 1 << 11,
539FORCEOUT_hinge2Angle = 1 << 12,
540FORCEOUT_hinge2AngleRate= 1 << 13,
541//SINGLEAXIS
542FORCEOUT_angle = 1 << 14,
543FORCEOUT_angleRate = 1 << 15,
544//SLIDER
545FORCEOUT_separation = 1 << 16,
546FORCEOUT_separationRate = 1 << 17,
547
548
549};
550static struct force_output_fieldname {
551char *fieldname;
552unsigned 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};
583unsigned 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
602void 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
798static int rbp_pause = FALSE;
799static double STEP_SIZE = .02; //seconds
800/* http://www.ode.org/ode-0.039-userguide.html#ref27
8013.10. Typical simulation code
802A typical simulation will proceed like this:
8031.Create a dynamics world.
8042.Create bodies in the dynamics world.
8053.Set the state (position etc) of all bodies.
8064.Create joints in the dynamics world.
8075.Attach the joints to the bodies.
8086.Set the parameters of all joints.
8097.Create a collision world and collision geometry objects, as necessary.
8108.Create a joint group to hold the contact joints.
8119.Loop:
8121. Apply forces to the bodies as necessary.
8132. Adjust the joint parameters as necessary.
8143. Call collision detection.
8154. Create a contact joint for every collision point, and put it in the contact joint group.
8165. Take a simulation step.
8176. Remove all joints in the contact joint group.
81810.Destroy the dynamics and collision worlds.
819
820UPDATING NODES when NODE_NEEDS_COMPILING:
821Usually when you want to change a parameter on a running physics simulation, you
822don't want the simulation to completely start over. You want it to carry on. The
823physics engine holds state for where things are.
824
825In freewrl > propagate_events_B() > update_node(toNode); we flag the whole target
826node as needing a recompile, even when only one field was changed.
827To tell later, when compile_ recompiling the target node, which fields have changed
828we change the PERL node struct to have __oldfieldvalue and each time we recompile
829we save the current value of the field. So next time we compile_ the node,
830we can see if newfield == __oldfieldvalue, and if not, that field changed - apply the change.
831When we don't check, its because no harm is done by re-setting the field even if not changed.
832
833For RBPhysics nodes, there are lots of fields that can be harmlessly reset in the
834middle of a simulation.
835But there are some fields that should only be reset if the field changed
836ORIC - Only Reset If Changed
837There's a pattern to them:
8381. 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
8412. 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
844I'll list (my guess at) the ORIC (Only Reset If Changed) fields here:
845
846BallJoint
847anchorPoint
848body1,body2
849
850CollidableShape,CollidableOffset
851(recompile like Transform node)
852x but don't flag parent X3DBody as needing to recompile
853
854DoubleAxisHingeJoint
855anchorPoint
856axis1,2
857body1,body2
858
859MotorJoint
860motor1Axis,motor2Axis,motor3Axis
861axisAngle1,axisAngle2,axisAngle3
862body1,body2
863
864RigidBody
865centerOfMass
866finiteRotationAxis
867linearVelocity
868angularVelocity
869orientation
870position
871
872SingleAxisHinge
873anchorPoint
874axis
875body1,body2
876
877SliderJoint
878axis
879body1,body2
880
881UniversalJoint
882anchorPoint
883axis1,2,3
884body1,body2
885
886Total: 31 ORIC fields
887(plus lots of other fields that are wastefully reset on a generic recompile)
888
889There are various options/possibilities/strategies for recording and recovering which fields were changed:
8901. __oldvalue fields for ORIC fields. Works well for scalars, but not MF/.n,.p fields (like forceOutput)
8912. 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)
8933. refactor freewrl code to include a bitflag in each field (like Script and Proto field structs)
8944. 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
899Issue 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
903DECISION: we'll do the #1 __oldvalue technique.
904
905*/
906int static_did_physics_since_last_Tick = FALSE;
907void 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;
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((double *)contact.geom.pos,ct->position.c,3);
1062 float2double((double *)contact.fdir1,ct->frictionDirection.c,2);
1063 float2double((double *)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
1934void register_CollisionCollection(struct X3D_Node * _node){
1935 if(_node->_nodeType == NODE_CollisionCollection){
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}
1945void register_CollisionSensor(struct X3D_Node *_node){
1946 if(_node->_nodeType == NODE_CollisionSensor){
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
1956void register_RigidBodyCollection(struct X3D_Node *_node){
1957 if(_node->_nodeType == NODE_RigidBodyCollection){
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
2015void 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}
2051void compile_CollidableShape(struct X3D_Node *_node){
2052 if(_node->_nodeType == NODE_CollidableShape || _node->_nodeType == NODE_CollidableOffset){
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
2064void 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}
2081void compile_CollidableOffset(struct X3D_Node *node){
2082 compile_CollidableShape(node);
2083}
2084void child_CollidableShape(struct X3D_Node *_node){
2085 if(_node->_nodeType == NODE_CollidableShape){
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}
2092void prep_CollidableOffset(struct X3D_Node *node){
2093 prep_CollidableShape(node);
2094}
2095void fin_CollidableOffset(struct X3D_Node *node){
2096 fin_CollidableShape(node);
2097}
2098void child_CollidableOffset(struct X3D_Node *_node){
2099 if(_node->_nodeType == NODE_CollidableOffset){
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
2126void 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}
2163void do_CollisionSensorTick(void * ptr){
2164 if(ptr)
2165 do_CollisionSensorTick0((struct X3D_CollisionSensor *)ptr);
2166}
2167
2168void 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
2197void compile_CollidableShape(struct X3D_Node *node){
2198}
2199void prep_CollidableShape(struct X3D_Node *node){
2200}
2201void fin_CollidableShape(struct X3D_Node *node){
2202}
2203void child_CollidableShape(struct X3D_Node *node){
2204}
2205void compile_CollidableOffset(struct X3D_Node *node){
2206}
2207void prep_CollidableOffset(struct X3D_Node *node){
2208}
2209void fin_CollidableOffset(struct X3D_Node *node){
2210}
2211void child_CollidableOffset(struct X3D_Node *node){
2212}
2213void rbp_run_physics(){
2214}
2215void add_physics(struct X3D_Node *node){
2216}
2217void do_CollisionSensorTick(void * ptr){
2218}
2219
2220#endif
2221
2222
2223
2224