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;
66}* ppComponent_RigidBodyPhysics;
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 {
77 ppComponent_RigidBodyPhysics p = (ppComponent_RigidBodyPhysics)t->prv;
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;
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") || !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") || !strcmp(ap, "SPEED_1")){
998 mask |= dContactMotion1;
999 }else if(!strcmp(ap,"SPEED-2") || !strcmp(ap, "SPEED_2")){
1000 mask |= dContactMotion2;
1001 }else if(!strcmp(ap,"SLIP-1") || !strcmp(ap, "SLIP_1")){
1002 mask |= dContactSlip1;
1003 }else if(!strcmp(ap,"SLIP-2") || !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 if (X3D_PROTO(jnt->_executionContext)->__specversion >= 400) {
1434 dJointSetHinge2Param(jnt->_joint, dParamBounce1, jnt->stop1Bounce);
1435 dJointSetHinge2Param(jnt->_joint, dParamStopERP1, jnt->stop1ErrorCorrection);
1436 }
1437 else {
1438 dJointSetHinge2Param(jnt->_joint, dParamBounce1, jnt->stopBounce1);
1439 dJointSetHinge2Param(jnt->_joint, dParamStopERP1, jnt->stopErrorCorrection1);
1440 }
1441 dJointSetHinge2Param (jnt->_joint,dParamSuspensionERP,jnt->suspensionErrorCorrection);
1442 dJointSetHinge2Param (jnt->_joint,dParamSuspensionCFM,jnt->suspensionForce);
1443
1444 dJointSetHinge2Param (jnt->_joint,dParamFMax,jnt->maxTorque1);
1445 dJointSetHinge2Param (jnt->_joint,dParamLoStop,jnt->minAngle1);
1446 dJointSetHinge2Param (jnt->_joint,dParamHiStop,jnt->maxAngle1);
1447 dJointSetHinge2Param (jnt->_joint,dParamFudgeFactor,0.1);
1448
1449 MNC(jnt);
1450 }
1451 //per-frame
1452 //motor
1453 dJointSetHinge2Param (jnt->_joint,dParamVel2,jnt->desiredAngularVelocity2);
1454 dJointSetHinge2Param (jnt->_joint,dParamFMax2,jnt->maxTorque2);
1455 //steering
1456 avel = jnt->desiredAngularVelocity1;
1457 method_extension_axis1Angle = 0;
1458 if(method_extension_axis1Angle){
1459 //the specs don't have an axis1Angle on DoubleAxisHingeJoint - too bad
1460 //because here is how easy it would be to steer a car
1461 //instead, you have to chain a motor to axis1
1462 double agap = jnt->axis1Angle - dJointGetHinge2Angle1 (jnt->_joint);
1463 //printf("agap %lf = %f - %lf\n",agap,jnt->axis1Angle,dJointGetHinge2Angle1 (jnt->_joint));
1464 double avel = agap / .1 * jnt->desiredAngularVelocity1;
1465 if (agap > 0.1) avel = jnt->desiredAngularVelocity1;
1466 if (agap < -0.1) avel = -jnt->desiredAngularVelocity1;
1467 }
1468 dJointSetHinge2Param (jnt->_joint,dParamVel,avel);
1469
1470 }
1471 break;
1472 case NODE_SliderJoint:
1473 {
1474 dJointID jointID;
1475 struct X3D_SliderJoint *jnt = (struct X3D_SliderJoint*)joint;
1476 if(!jnt->_joint){
1477 dBodyID body1ID, body2ID;
1478 struct X3D_RigidBody *xbody1, *xbody2;
1479 xbody1 = (struct X3D_RigidBody*)jnt->body1;
1480 xbody2 = (struct X3D_RigidBody*)jnt->body2;
1481 //allow for MULL body on one side of joint, to fix to static environment
1482 body1ID = xbody1 ? xbody1->_body : NULL;
1483 body2ID = xbody2 ? xbody2->_body : NULL;
1484 jointID = dJointCreateSlider (x3dworld->_world,x3dworld->_group);
1485 dJointAttach (jointID,body1ID,body2ID);
1486 jnt->_joint = jointID;
1487 }
1488 if(NNC(jnt)){
1489 float axislen = veclength3f(jnt->axis.c);
1490 if(axislen < .1){
1491 //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1492 jnt->axis.c[0] = jnt->axis.c[1] = 0.0f;
1493 jnt->axis.c[2] = 1.0f;
1494 }
1495 if(!vecsame3f(jnt->__old_axis.c,jnt->axis.c)){
1496 dJointSetSliderAxis (jnt->_joint,jnt->axis.c[0],jnt->axis.c[1],jnt->axis.c[2]);
1497 veccopy3f(jnt->__old_axis.c,jnt->axis.c);
1498 }
1499 jnt->_forceout = forceout_from_names(jnt->forceOutput.n,jnt->forceOutput.p);
1500 dJointSetSliderParam (jnt->_joint,dParamBounce,jnt->stopBounce);
1501 dJointSetSliderParam (jnt->_joint,dParamStopERP,jnt->stopErrorCorrection);
1502
1503 MNC(jnt);
1504 }
1505 }
1506 break;
1507 case NODE_UniversalJoint:
1508 {
1509 // http://ode.org/ode-latest-userguide.html#sec_7_3_4
1510 // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html#UniversalJoint
1511 dJointID jointID;
1512 struct X3D_UniversalJoint *jnt = (struct X3D_UniversalJoint*)joint;
1513 if(!jnt->_joint){
1514 dBodyID body1ID, body2ID;
1515 struct X3D_RigidBody *xbody1, *xbody2;
1516 xbody1 = (struct X3D_RigidBody*)jnt->body1;
1517 xbody2 = (struct X3D_RigidBody*)jnt->body2;
1518 //allow for MULL body on one side of joint, to fix to static environment
1519 body1ID = xbody1 ? xbody1->_body : NULL;
1520 body2ID = xbody2 ? xbody2->_body : NULL;
1521 jointID = dJointCreateUniversal (x3dworld->_world,x3dworld->_group);
1522 dJointAttach (jointID,body1ID,body2ID);
1523 jnt->_joint = jointID;
1524 //anchor point can be 0 0 0. If so, our vecsame3f technique below fails to set UniversalAnchor at least once.
1525 //so we do the zero-capable fields in the _joint section once
1526 dJointSetUniversalAnchor (jnt->_joint,jnt->anchorPoint.c[0],jnt->anchorPoint.c[1],jnt->anchorPoint.c[2]);
1527 }
1528 if(NNC(jnt)){
1529 float axislen;
1530 if(!vecsame3f(jnt->__old_anchorPoint.c,jnt->anchorPoint.c)){
1531 dJointSetUniversalAnchor (jnt->_joint,jnt->anchorPoint.c[0],jnt->anchorPoint.c[1],jnt->anchorPoint.c[2]);
1532 veccopy3f(jnt->__old_anchorPoint.c,jnt->anchorPoint.c);
1533 }
1534
1535 axislen = veclength3f(jnt->axis1.c);
1536 if(axislen < .1){
1537 //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1538 jnt->axis1.c[0] = jnt->axis1.c[1] = 0.0f;
1539 jnt->axis1.c[2] = 1.0f;
1540 }
1541 axislen = veclength3f(jnt->axis2.c);
1542 if(axislen < .1){
1543 //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1544 jnt->axis2.c[0] = jnt->axis2.c[2] = 0.0f;
1545 jnt->axis2.c[1] = 1.0f;
1546 }
1547 if(!vecsame3f(jnt->__old_axis1.c,jnt->axis1.c)){
1548 dJointSetUniversalAxis1 (jnt->_joint,jnt->axis1.c[0],jnt->axis1.c[1],jnt->axis1.c[2]);
1549 veccopy3f(jnt->__old_axis1.c,jnt->axis1.c);
1550 }
1551 if(!vecsame3f(jnt->__old_axis2.c,jnt->axis2.c)){
1552 dJointSetUniversalAxis2(jnt->_joint,jnt->axis2.c[0],jnt->axis2.c[1],jnt->axis2.c[2]);
1553 veccopy3f(jnt->__old_axis2.c,jnt->axis2.c);
1554 }
1555 jnt->_forceout = forceout_from_names(jnt->forceOutput.n,jnt->forceOutput.p);
1556 dJointSetUniversalParam (jnt->_joint,dParamBounce1,jnt->stop1Bounce);
1557 dJointSetUniversalParam (jnt->_joint,dParamStopERP1,jnt->stop1ErrorCorrection);
1558 dJointSetUniversalParam (jnt->_joint,dParamBounce2,jnt->stop2Bounce);
1559 dJointSetUniversalParam (jnt->_joint,dParamStopERP2,jnt->stop2ErrorCorrection);
1560
1561 MNC(jnt);
1562 }
1563 }
1564 break;
1565 case NODE_MotorJoint:
1566 {
1567 // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html#MotorJoint
1568 // x3dmotorjoint only ode Angular AMotor, (no Linear LMotor parameters)
1569 // only ode so-called User mode
1570 // http://ode.org/ode-latest-userguide.html#sec_7_5_1
1571 // table of joint parameters
1572 // dParamVel and dParamFMax relate specifically to AMotor joints
1573 dJointID jointID;
1574 struct X3D_MotorJoint *jnt = (struct X3D_MotorJoint*)joint;
1575 if(!jnt->_joint){
1576 dBodyID body1ID, body2ID;
1577 struct X3D_RigidBody *xbody1, *xbody2;
1578 xbody1 = (struct X3D_RigidBody*)jnt->body1;
1579 xbody2 = (struct X3D_RigidBody*)jnt->body2;
1580 //allow for MULL body on one side of joint, to fix to static environment
1581 body1ID = xbody1 ? xbody1->_body : NULL;
1582 body2ID = xbody2 ? xbody2->_body : NULL;
1583 jointID = dJointCreateAMotor (x3dworld->_world,x3dworld->_group);
1584 dJointAttach (jointID,body1ID,body2ID);
1585 jnt->_joint = jointID;
1586 dJointSetAMotorMode (jnt->_joint,dAMotorUser);
1587 //dJointSetAMotorMode (jointID,dAMotorEuler);
1588 }
1589 if(NNC(jnt)){
1590 float axislen;
1591 dJointSetAMotorNumAxes (jnt->_joint,jnt->enabledAxes);
1592 //rel: relative to: 0-static 1-first body 2-2nd body
1593 if(jnt->enabledAxes >0 ){
1594 axislen = veclength3f(jnt->motor1Axis.c);
1595 if(axislen < .1){
1596 //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1597 jnt->motor1Axis.c[0] = jnt->motor1Axis.c[1] = 0.0f;
1598 jnt->motor1Axis.c[2] = 1.0f;
1599 }
1600
1601 if(!vecsame3f(jnt->__old_motor1Axis.c,jnt->motor1Axis.c)){
1602 dJointSetAMotorAxis (jnt->_joint,0,0, jnt->motor1Axis.c[0],jnt->motor1Axis.c[1],jnt->motor1Axis.c[2]);
1603 veccopy3f(jnt->__old_motor1Axis.c,jnt->motor1Axis.c);
1604 }
1605 if(jnt->__old_axis1Angle != jnt->axis1Angle){
1606 dJointSetAMotorAngle (jnt->_joint, 0, jnt->axis1Angle);
1607 jnt->__old_axis1Angle = jnt->axis1Angle;
1608 }
1609 }
1610 if(jnt->enabledAxes >1 ){
1611 axislen = veclength3f(jnt->motor2Axis.c);
1612 if(axislen < .1){
1613 //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1614 jnt->motor2Axis.c[0] = jnt->motor2Axis.c[1] = 0.0f;
1615 jnt->motor2Axis.c[2] = 1.0f;
1616 }
1617
1618 if(!vecsame3f(jnt->__old_motor2Axis.c,jnt->motor2Axis.c)){
1619 dJointSetAMotorAxis (jnt->_joint,1,1, jnt->motor2Axis.c[0],jnt->motor2Axis.c[1],jnt->motor2Axis.c[2]);
1620 veccopy3f(jnt->__old_motor2Axis.c,jnt->motor2Axis.c);
1621 }
1622 if(jnt->__old_axis2Angle != jnt->axis2Angle){
1623 dJointSetAMotorAngle (jnt->_joint, 1, jnt->axis2Angle);
1624 jnt->__old_axis2Angle = jnt->axis2Angle;
1625 }
1626 //dJointSetAMotorParam(jointID,dParamFMax2,jnt->axis2Torque);
1627 }
1628 if(jnt->enabledAxes >2 ){
1629 axislen = veclength3f(jnt->motor3Axis.c);
1630 if(axislen < .1){
1631 //specs say 0 0 0 is default but that's garbage, should be 0 0 1
1632 jnt->motor3Axis.c[0] = jnt->motor3Axis.c[1] = 0.0f;
1633 jnt->motor3Axis.c[2] = 1.0f;
1634 }
1635
1636 if(!vecsame3f(jnt->__old_motor3Axis.c,jnt->motor3Axis.c)){
1637 dJointSetAMotorAxis (jnt->_joint,2,2, jnt->motor3Axis.c[0],jnt->motor3Axis.c[1],jnt->motor3Axis.c[2]);
1638 veccopy3f(jnt->__old_motor3Axis.c,jnt->motor3Axis.c);
1639 }
1640 if(jnt->__old_axis3Angle != jnt->axis3Angle){
1641 dJointSetAMotorAngle (jnt->_joint, 2, jnt->axis3Angle);
1642 jnt->__old_axis3Angle = jnt->axis3Angle;
1643 }
1644 //dJointSetAMotorParam(jointID,dParamFMax3,jnt->axis3Torque);
1645 }
1646 jnt->_forceout = forceout_from_names(jnt->forceOutput.n,jnt->forceOutput.p);
1647
1648 dJointSetAMotorParam (jnt->_joint,dParamBounce1,jnt->stop1Bounce);
1649 dJointSetAMotorParam (jnt->_joint,dParamStopERP1,jnt->stop1ErrorCorrection);
1650 dJointSetAMotorParam (jnt->_joint,dParamBounce2,jnt->stop2Bounce);
1651 dJointSetAMotorParam (jnt->_joint,dParamStopERP2,jnt->stop2ErrorCorrection);
1652 dJointSetAMotorParam (jnt->_joint,dParamBounce3,jnt->stop3Bounce);
1653 dJointSetAMotorParam (jnt->_joint,dParamStopERP3,jnt->stop3ErrorCorrection);
1654
1655 //addMotorTorques is a macro function in ODE that finds the bodies involve and
1656 //applies the torques to the bodies
1657 //if(jnt->autoCalc == TRUE)
1658 dJointAddAMotorTorques(jnt->_joint, jnt->axis1Torque, jnt->axis2Torque, jnt->axis3Torque);
1659 MNC(jnt);
1660
1661 }
1662 //per-frame torque - this will cause an acceleration, don't know if that's
1663 //what the x3dmotorjoint torque fields were meant for
1664 //dJointAddAMotorTorques(jnt->_joint, jnt->axis1Torque, jnt->axis2Torque, jnt->axis3Torque);
1665 if(jnt->autoCalc == FALSE){
1666 //user sets angles on each frame
1667 if(jnt->enabledAxes >0)
1668 dJointSetAMotorAngle (jnt->_joint, 0, jnt->axis1Angle);
1669 if(jnt->enabledAxes >1)
1670 dJointSetAMotorAngle (jnt->_joint, 1, jnt->axis2Angle);
1671 if(jnt->enabledAxes >2)
1672 dJointSetAMotorAngle (jnt->_joint, 2, jnt->axis3Angle);
1673 }
1674 }
1675 break;
1676 default:
1677 break;
1678 } //switch (joint type)
1679 }
1680
1681 //RUN PHYSICS ENGINE
1682 dSpaceCollide (space,0,&nearCallback);
1683 if (!rbp_pause) {
1684 if(x3dworld->preferAccuracy){
1685 //PUNISHING GIANT MATRIX PUSHED ONTO C STACK FOR GIANT MATRIX SOLUTION
1686 double step_fraction = STEP_SIZE;
1687 dWorldStep (x3dworld->_world,step_fraction);
1688 }else{
1689 //SO CALLED QUICK-STEP METHOD WHICH IS FASTER, THE NORMAL WAY TO DO IT
1690 //dWorldSetQuickStepNumIterations (x3dworld->_world, x3dworld->iterations);
1691 double step_fraction = STEP_SIZE / (double)x3dworld->iterations;
1692 for(nstep=0;nstep<x3dworld->iterations;nstep++)
1693 dWorldQuickStep (x3dworld->_world,step_fraction);
1694 }
1695 }
1696
1697 //do eventOuts
1698 //Rigidbody -> Collidable
1699 for(i=0;i<x3dworld->bodies.n;i++){
1700 x3dbody = (struct X3D_RigidBody*)x3dworld->bodies.p[i];
1701 if(x3dbody->_body){
1702 //if not fixed, it will have a body that maybe moved
1703 //ATTEMPT 5 and 6
1704 //we set and mark both x3dbody and top-level collidable translation,rotation
1705 //top level collidable: we concatonate x3dboy * top-collidable transform
1706 const dReal *dpos, *dquat;
1707 //dReal *drot;
1708 Quaternion quat;
1709 double xyza[4];
1710
1711 dpos = dBodyGetPosition (x3dbody->_body);
1712 //printf("dpos = %lf %lf %lf\n",dpos[0],dpos[1],dpos[2]);
1713 dquat = dBodyGetQuaternion(x3dbody->_body);
1714 quat.x = dquat[1], quat.y = dquat[2], quat.z = dquat[3], quat.w = dquat[0];
1715 quaternion_to_vrmlrot(&quat,&xyza[0],&xyza[1],&xyza[2],&xyza[3]);
1716 //double2float(x3dbody->position.c,dpos,3);
1717 x3dbody->position.c[0] = dpos[0];x3dbody->position.c[1] = dpos[1];x3dbody->position.c[2] = dpos[2];
1718 double2float(x3dbody->orientation.c,xyza,4);
1719 MARK_EVENT(X3D_NODE(x3doffset),offsetof(struct X3D_RigidBody,position));
1720 MARK_EVENT(X3D_NODE(x3doffset),offsetof(struct X3D_RigidBody,orientation));
1721
1722 for(k=0;k<x3dbody->geometry.n;k++){
1723 float *translation, *rotation;
1724 struct X3D_CollidableOffset* x3doffset = (struct X3D_CollidableOffset*)x3dbody->geometry.p[k];
1725 translation = x3doffset->translation.c;
1726 rotation = x3doffset->rotation.c;
1727 //ATTEMPT 5: concatonate rigidbody transform with top-level collidable transform
1728 if(x3doffset->_nodeType == NODE_CollidableOffset){
1729 //body_T * body_R * geom_T * geom_R ==
1730 //(body_T + body_R*geom_T) * body_R*geom R
1731 double geomT[3], geomR[4];
1732 Quaternion geomQ;
1733 float2double(geomT,x3doffset->_initialTranslation.c,3);
1734 float2double(geomR,x3doffset->_initialRotation.c,4);
1735 vrmlrot_to_quaternion(&geomQ,geomR[0],geomR[1],geomR[2],geomR[3]);
1736 quaternion_rotationd(geomT,&quat,geomT);
1737 double2float(translation,geomT,3);
1738 vecadd3f(translation,translation,x3dbody->position.c);
1739 quaternion_multiply(&geomQ,&quat,&geomQ);
1740 quaternion_to_vrmlrot(&geomQ,&geomR[0],&geomR[1],&geomR[2],&geomR[3]);
1741 double2float(rotation,geomR,4);
1742 }
1743 if(x3doffset->_nodeType == NODE_CollidableShape){
1744 veccopy3f(translation,x3dbody->position.c);
1745 veccopy4f(rotation,x3dbody->orientation.c);
1746 }
1747 //double2float(translation,dpos,3);
1748 //double2float(rotation,xyza,4);
1749 if(0) if(i==0){
1750 //some debug
1751 static int loopcount = 0;
1752 if(loopcount < 30){
1753 printf("pos %lf %lf %lf\n",dpos[0],dpos[1],dpos[2]);
1754 printf("rot %f %f %f %f\n",rotation[0],rotation[1],rotation[2],rotation[3]);
1755 printf("trn %f %f %f\n",translation[0],translation[1],translation[2]);
1756 loopcount++;
1757 }
1758 }
1759 //if(1) printf("transout %f %f %f\n",translation[0],translation[1],translation[2]);
1760 MARK_EVENT(X3D_NODE(x3doffset),offsetof(struct X3D_CollidableOffset,translation));
1761 MARK_EVENT(X3D_NODE(x3doffset),offsetof(struct X3D_CollidableOffset,rotation));
1762
1763 x3doffset->_change++;
1764 }
1765 }
1766 } //for bodies
1767 for(i=0;i<x3dworld->joints.n;i++){
1768 struct X3D_Node *joint = (struct X3D_Node*)x3dworld->joints.p[i];
1769 // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html#X3DRigidJointNode
1770 // forceOutput = [NONE] ["NONE","ALL", specific output field name ie for MotorJoint "motor1AngleRate"
1771 switch(joint->_nodeType){
1772 case NODE_BallJoint:
1773 {
1774 //dJointID jointID;
1775 struct X3D_BallJoint *jnt = (struct X3D_BallJoint*)joint;
1776 if(jnt->_forceout){
1777 if(jnt->_forceout & FORCEOUT_body1AnchorPoint){
1778 // jnt->body1AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1779 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_BallJoint,body1AnchorPoint));
1780 }
1781 if(jnt->_forceout & FORCEOUT_body2AnchorPoint){
1782 // jnt->body2AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1783 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_BallJoint,body2AnchorPoint));
1784 }
1785
1786 }
1787 }
1788 break;
1789 case NODE_SingleAxisHingeJoint:
1790 {
1791 //dJointID jointID;
1792 struct X3D_SingleAxisHingeJoint *jnt = (struct X3D_SingleAxisHingeJoint*)joint;
1793 if(jnt->_forceout){
1794
1795 if(jnt->_forceout & FORCEOUT_angle){
1796 // jnt->angle = dJointGetAMotorAngle( jnt->_joint, 0 );
1797 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SingleAxisHingeJoint,angle));
1798 }
1799 if(jnt->_forceout & FORCEOUT_angleRate){
1800 // jnt->angleRate = dJointGetAMotorAngle( jnt->_joint, 0 );
1801 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SingleAxisHingeJoint,angleRate));
1802 }
1803
1804 if(jnt->_forceout & FORCEOUT_body1AnchorPoint){
1805 // jnt->body1AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1806 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SingleAxisHingeJoint,body1AnchorPoint));
1807 }
1808 if(jnt->_forceout & FORCEOUT_body2AnchorPoint){
1809 // jnt->body2AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1810 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SingleAxisHingeJoint,body2AnchorPoint));
1811 }
1812 }
1813 }
1814 break;
1815 case NODE_DoubleAxisHingeJoint:
1816 {
1817 //dJointID jointID;
1818 struct X3D_DoubleAxisHingeJoint *jnt = (struct X3D_DoubleAxisHingeJoint*)joint;
1819 if(jnt->_forceout){
1820 if(jnt->_forceout & FORCEOUT_body1AnchorPoint){
1821 // jnt->body1AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1822 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,body1AnchorPoint));
1823 }
1824 if(jnt->_forceout & FORCEOUT_body1Axis){
1825 // jnt->body1Axis = dJointGetAMotorAngle( jnt->_joint, 0 );
1826 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,body1Axis));
1827 }
1828 if(jnt->_forceout & FORCEOUT_body2AnchorPoint){
1829 // jnt->body2AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1830 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,body2AnchorPoint));
1831 }
1832 if(jnt->_forceout & FORCEOUT_body2Axis){
1833 // jnt->body2Axis = dJointGetAMotorAngle( jnt->_joint, 0 );
1834 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,body2Axis));
1835 }
1836 if(jnt->_forceout & FORCEOUT_hinge1Angle){
1837 // jnt->hinge1Angle = dJointGetAMotorAngle( jnt->_joint, 0 );
1838 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,hinge1Angle));
1839 }
1840 if(jnt->_forceout & FORCEOUT_hinge1AngleRate){
1841 // jnt->hinge1AngleRate = dJointGetAMotorAngle( jnt->_joint, 0 );
1842 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,hinge1AngleRate));
1843 }
1844 if(jnt->_forceout & FORCEOUT_hinge2Angle){
1845 // jnt->hinge2Angle = dJointGetAMotorAngle( jnt->_joint, 0 );
1846 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,hinge2Angle));
1847 }
1848 if(jnt->_forceout & FORCEOUT_hinge2AngleRate){
1849 // jnt->hinge2AngleRate = dJointGetAMotorAngle( jnt->_joint, 0 );
1850 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_DoubleAxisHingeJoint,hinge2AngleRate));
1851 }
1852
1853 }
1854 }
1855 break;
1856 case NODE_SliderJoint:
1857 {
1858 //dJointID jointID;
1859 struct X3D_SliderJoint *jnt = (struct X3D_SliderJoint*)joint;
1860 if(jnt->_forceout){
1861 if(jnt->_forceout & FORCEOUT_separation){
1862 // jnt->separation = dJointGetAMotorAngle( jnt->_joint, 0 );
1863 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SliderJoint,separation));
1864 }
1865 if(jnt->_forceout & FORCEOUT_separationRate){
1866 // jnt->separationRate = dJointGetAMotorAngle( jnt->_joint, 0 );
1867 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_SliderJoint,separationRate));
1868 }
1869 }
1870 }
1871 break;
1872 case NODE_UniversalJoint:
1873 {
1874 //dJointID jointID;
1875 struct X3D_UniversalJoint *jnt = (struct X3D_UniversalJoint*)joint;
1876 if(jnt->_forceout){
1877 if(jnt->_forceout & FORCEOUT_body1AnchorPoint){
1878 // jnt->body1AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1879 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_UniversalJoint,body1AnchorPoint));
1880 }
1881 if(jnt->_forceout & FORCEOUT_body1Axis){
1882 // jnt->body1Axis = dJointGetAMotorAngle( jnt->_joint, 0 );
1883 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_UniversalJoint,body1Axis));
1884 }
1885 if(jnt->_forceout & FORCEOUT_body2AnchorPoint){
1886 // jnt->body2AnchorPoint = dJointGetAMotorAngle( jnt->_joint, 0 );
1887 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_UniversalJoint,body2AnchorPoint));
1888 }
1889 if(jnt->_forceout & FORCEOUT_body2Axis){
1890 // jnt->body2Axis = dJointGetAMotorAngle( jnt->_joint, 0 );
1891 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_UniversalJoint,body2Axis));
1892 }
1893 }
1894 }
1895 break;
1896 case NODE_MotorJoint:
1897 {
1898 //dJointID jointID;
1899 struct X3D_MotorJoint *jnt = (struct X3D_MotorJoint*)joint;
1900 if(jnt->_forceout){
1901 if(jnt->_forceout & FORCEOUT_motor1Angle){
1902 jnt->motor1Angle = (float) dJointGetAMotorAngle( jnt->_joint, 0 );
1903 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor1Angle));
1904 }
1905 if(jnt->_forceout & FORCEOUT_motor1AngleRate){
1906 jnt->motor1AngleRate = (float) dJointGetAMotorAngleRate( jnt->_joint, 0 );
1907 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor1AngleRate));
1908 }
1909 if(jnt->_forceout & FORCEOUT_motor2Angle){
1910 jnt->motor2Angle = (float) dJointGetAMotorAngle( jnt->_joint, 1 );
1911 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor2Angle));
1912 }
1913 if(jnt->_forceout & FORCEOUT_motor2AngleRate){
1914 jnt->motor2AngleRate = (float) dJointGetAMotorAngleRate( jnt->_joint, 1 );
1915 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor2AngleRate));
1916 }
1917 if(jnt->_forceout & FORCEOUT_motor3Angle){
1918 jnt->motor3Angle = (float) dJointGetAMotorAngle( jnt->_joint, 2 );
1919 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor3Angle));
1920 }
1921 if(jnt->_forceout & FORCEOUT_motor3AngleRate){
1922 jnt->motor3AngleRate = (float) dJointGetAMotorAngleRate( jnt->_joint, 2 );
1923 MARK_EVENT(X3D_NODE(jnt),offsetof(struct X3D_MotorJoint,motor3AngleRate));
1924 }
1925 }
1926
1927 }
1928 break;
1929 default:
1930 break;
1931 } //switch on joint
1932 } //for joints
1933 // remove all contact joints
1934 dJointGroupEmpty (contactgroup);
1935 }
1936 }
1937
1938}
1939
1940void register_CollisionCollection(struct X3D_Node * _node){
1941 if(_node->_nodeType == NODE_CollisionCollection){
1942 ppComponent_RigidBodyPhysics p;
1943 struct X3D_CollisionSensor *node = (struct X3D_CollisionSensor*)_node;
1944 p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
1945 if(!x3dcollisioncollections) x3dcollisioncollections = newVector(struct X3D_Node*,5);
1946 if(!space) dHashSpaceCreate(0); //default space
1947 vector_pushBack(struct X3D_Node*,x3dcollisioncollections,_node);
1948 MARK_NODE_COMPILED;
1949 }
1950}
1951void register_CollisionSensor(struct X3D_Node *_node){
1952 if(_node->_nodeType == NODE_CollisionSensor){
1953 ppComponent_RigidBodyPhysics p;
1954 struct X3D_CollisionSensor *node = (struct X3D_CollisionSensor*)_node;
1955 p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
1956 if(!x3dcollisionsensors) x3dcollisionsensors = newVector(struct X3D_Node*,5);
1957 vector_pushBack(struct X3D_Node*,x3dcollisionsensors,_node);
1958 MARK_NODE_COMPILED;
1959 }
1960}
1961
1962void register_RigidBodyCollection(struct X3D_Node *_node){
1963 if(_node->_nodeType == NODE_RigidBodyCollection){
1964 ppComponent_RigidBodyPhysics p;
1965 dJointGroupID groupID;
1966 struct X3D_RigidBodyCollection *node = (struct X3D_RigidBodyCollection*)_node;
1967 p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
1968
1969 if(!space)
1970 space = dHashSpaceCreate (0); //default space
1971 if(!world){
1972 world = dWorldCreate();
1973 dWorldSetGravity (world,node->gravity.c[0],node->gravity.c[1],node->gravity.c[2]);
1974 dWorldSetCFM (world,1e-5);
1975 dWorldSetAutoDisableFlag (world,1);
1976 //this contactgroup is a default group for doing just collisions, cleared every frame:
1977 // 1)collide (callback to make temporary joints out of contact points)
1978 // 2)physics step (uses all joints)
1979 // 3)clear contactgroup (clean out all the temporary collision joints)
1980 contactgroup = dJointGroupCreate (0);
1981
1982 #if 1
1983
1984 dWorldSetAutoDisableAverageSamplesCount( world, 10 );
1985
1986 #endif
1987
1988 dWorldSetLinearDamping(world, 0.00001);
1989 dWorldSetAngularDamping(world, 0.005);
1990 dWorldSetMaxAngularSpeed(world, 200);
1991
1992 dWorldSetContactMaxCorrectingVel (world,0.1);
1993 dWorldSetContactSurfaceLayer (world,0.001);
1994 }
1996 //node->_space = (void*)space;
1997 groupID = dJointGroupCreate (0); //this is a contact group for joints
1998 node->_group = groupID;
1999 if(!x3dworlds) x3dworlds = newVector(struct X3D_Node*,5);
2000 vector_pushBack(struct X3D_Node*,x3dworlds,_node);
2001 MARK_NODE_COMPILED;
2002 }
2003}
2004
2005// http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/components/rigid_physics.html#TransformationHierarchy
2006// "Nodes .. are not part of the transform hierarchy.."
2007// 37.4.3 CollidableShape: "When placed under a part of the transformation hierarchy,
2008// it can be used to visually represent the movement of the object."
2009// Interpretation:
2010// if/when put outside of the X3DRigidBodyCollection -via DEF / USE- then
2011// the collidableshape (and any wrapper collidableOffset) transforms are pushed onto the
2012// render_hier transform stack and the (collision/physics proxy) Shape is rendered
2013// (versus if hidden with Switch -1, or only DEFed inside X3DRigidBodyCollection, Shape is never
2014// rendered, but CollidableShape and CollidableOffset should output translation_changed,
2015// and rotation_changed events, so can route to transforms containing non-proxy shapes
2016
2017// option: just expose compile and render virt_ functions for collidableshape and collidableoffset
2018//- and handle prep/fin/child privately
2019
2020
2021void prep_CollidableShape(struct X3D_Node *_node){
2022 struct X3D_CollidableShape *node = (struct X3D_CollidableShape*)_node;
2023 if(node){
2024 COMPILE_IF_REQUIRED
2025
2026 //CollidableOffset will come in here too, so in perl, keep the first fields in same order
2027 //so we can caste to one, and the fields jive
2028 if(!renderstate()->render_vp) {
2029
2030 /* rendering the viewpoint means doing the inverse transformations in reverse order (while poping stack),
2031 * so we do nothing here in that case -ncoder */
2032
2033 /* printf ("prep_Transform, render_hier vp %d geom %d light %d sens %d blend %d prox %d col %d\n",
2034 render_vp,render_geom,render_light,render_sensitive,render_blend,render_proximity,render_collision); */
2035
2036 /* do we have any geometry visible, and are we doing anything with geometry? */
2037 OCCLUSIONTEST
2038
2039 if(!renderstate()->render_vp) {
2040 /* do we actually have any thing to rotate/translate/scale?? */
2041
2042 FW_GL_PUSH_MATRIX();
2043
2044 /* TRANSLATION */
2045 if (node->__do_trans)
2046 FW_GL_TRANSLATE_F(node->translation.c[0],node->translation.c[1],node->translation.c[2]);
2047
2048 /* ROTATION */
2049 if (node->__do_rotation) {
2050 FW_GL_ROTATE_RADIANS(node->rotation.c[3], node->rotation.c[0],node->rotation.c[1],node->rotation.c[2]);
2051 }
2052 }
2053 }
2054 }
2055}
2056void compile_CollidableShape(struct X3D_Node *_node){
2057 if(_node->_nodeType == NODE_CollidableShape || _node->_nodeType == NODE_CollidableOffset){
2058 ppComponent_RigidBodyPhysics p;
2059 struct X3D_CollidableShape *node = (struct X3D_CollidableShape*)_node;
2060 p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
2061
2062 INITIALIZE_EXTENT;
2063 node->__do_trans = verify_translate ((GLfloat *)node->translation.c);
2064 node->__do_rotation = verify_rotate ((GLfloat *)node->rotation.c);
2065 MARK_NODE_COMPILED;
2066 }
2067}
2068
2069void fin_CollidableShape(struct X3D_Node *_node){
2070 if(_node){
2071 if(!renderstate()->render_vp) {
2072 FW_GL_POP_MATRIX();
2073 } else {
2074 /*Rendering the viewpoint only means finding it, and calculating the reverse WorldView matrix.*/
2075 if((_node->_renderFlags & VF_Viewpoint) == VF_Viewpoint) {
2076 struct X3D_CollidableShape *node = (struct X3D_CollidableShape*)_node;
2077
2078 FW_GL_ROTATE_RADIANS(-(((node->rotation).c[3])),((node->rotation).c[0]),((node->rotation).c[1]),((node->rotation).c[2])
2079 );
2080 FW_GL_TRANSLATE_F(-(((node->translation).c[0])),-(((node->translation).c[1])),-(((node->translation).c[2]))
2081 );
2082 }
2083 }
2084 }
2085}
2086void compile_CollidableOffset(struct X3D_Node *node){
2087 compile_CollidableShape(node);
2088}
2089void child_CollidableShape(struct X3D_Node *_node){
2090 if(_node->_nodeType == NODE_CollidableShape){
2091 ppComponent_RigidBodyPhysics p;
2092 struct X3D_CollidableShape *node = (struct X3D_CollidableShape*)_node;
2093 p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
2094 if(node->shape) render_node(node->shape);
2095 }
2096}
2097void prep_CollidableOffset(struct X3D_Node *node){
2098 prep_CollidableShape(node);
2099}
2100void fin_CollidableOffset(struct X3D_Node *node){
2101 fin_CollidableShape(node);
2102}
2103void child_CollidableOffset(struct X3D_Node *_node){
2104 if(_node->_nodeType == NODE_CollidableOffset){
2105 ppComponent_RigidBodyPhysics p;
2106 struct X3D_CollidableOffset *node = (struct X3D_CollidableOffset*)_node;
2107 p = (ppComponent_RigidBodyPhysics)gglobal()->Component_RigidBodyPhysics.prv;
2108
2109 //->collidable is an SFNode. Options:
2110 // a) we could go through normalChildren(n=1,p=&collidable) which would call prep and fin and child for us
2111 // b) we can call prep_, child_ and fin_ ourselves
2112 if(node->collidable) {
2113 switch(node->collidable->_nodeType){
2114 case NODE_CollidableOffset:
2115 prep_CollidableOffset(node->collidable);
2116 child_CollidableOffset(node->collidable);
2117 fin_CollidableOffset(node->collidable);
2118 break;
2119 case NODE_CollidableShape:
2120 prep_CollidableShape(node->collidable);
2121 child_CollidableShape(node->collidable);
2122 fin_CollidableShape(node->collidable);
2123 break;
2124 default:
2125 break;
2126 }
2127 }
2128 }
2129}
2130
2131void do_CollisionSensorTick0(struct X3D_CollisionSensor *node){
2132 //if we call this from the routing do_...Tick
2133 //that happens before rbp_run_phsyics() is called
2134 //so in rbp_run_physics the first thing we can do is clear the contacts in any sensors
2135 //then any contacts generated during physics can come out of here in the next routing session
2136 if(!static_did_physics_since_last_Tick) return;
2137 static_did_physics_since_last_Tick = 0;
2138 if(!node->enabled) return;
2139
2140 if(node->contacts.n){
2141 if(node->isActive == FALSE){
2142 node->isActive = TRUE;
2143 MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,isActive));
2144 }
2145 MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,contacts));
2146 //node->contacts.n = 0;
2147 // leave at 100 FREE_IF_NZ(node->contacts.p);
2148 }else{
2149 if(node->isActive == TRUE){
2150 node->isActive = FALSE;
2151 MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,isActive));
2152 //do I need to route n=0?
2153 //leave at 100 node->contacts.p = NULL; //if I don't set to null and mark, then CRoutes bombs in some cleanup code.
2154 //MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,contacts));
2155 }
2156 }
2157 if(node->isActive){
2158 if(node->intersections.n){
2159 MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,intersections));
2160 //node->intersections.n = 0;
2161 //leave at 100 FREE_IF_NZ(node->intersections.p);
2162 }
2163 //else{
2164 // MARK_EVENT(X3D_NODE(node),offsetof(struct X3D_CollisionSensor,intersections));
2165 //}
2166 }
2167}
2168void do_CollisionSensorTick(void * ptr){
2169 if(ptr)
2170 do_CollisionSensorTick0((struct X3D_CollisionSensor *)ptr);
2171}
2172
2173void add_physics(struct X3D_Node *node){
2174 switch(node->_nodeType){
2175 case NODE_CollisionSensor:
2176 //Nov. 2016: H: this should be in do_first ie do_CollisionSensor
2177 //which gets called before routing ie so if there's a Contact it can be routed
2178 //its almost the same place as physics, which is just after do_events,routing
2179 // http://www.web3d.org/documents/specifications/19775-1/V3.3/Part01/concepts.html#ExecutionModel
2180 register_CollisionSensor(node);
2181 break;
2182 case NODE_CollisionCollection:
2183 //CollisionCollections are x3dchild nodes, and can appear naked in scenegraph
2184 //or in CollisionSensor field, or in RigidBodyCollection field
2185 //ie might be DEF/USED
2186 register_CollisionCollection(node);
2187 break;
2188 case NODE_RigidBodyCollection:
2189 //OK good.
2190 register_RigidBodyCollection(node);
2191 break;
2192 //Q. what about CollisionSpace, CollisionCollection - should they be registered here?
2193 default:
2194 break;
2195 }
2196}
2197
2198//END MIT LIC <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
2199
2200#else //else no ode phyiscs engine, just stubs
2201
2202void compile_CollidableShape(struct X3D_Node *node){
2203}
2204void prep_CollidableShape(struct X3D_Node *node){
2205}
2206void fin_CollidableShape(struct X3D_Node *node){
2207}
2208void child_CollidableShape(struct X3D_Node *node){
2209}
2210void compile_CollidableOffset(struct X3D_Node *node){
2211}
2212void prep_CollidableOffset(struct X3D_Node *node){
2213}
2214void fin_CollidableOffset(struct X3D_Node *node){
2215}
2216void child_CollidableOffset(struct X3D_Node *node){
2217}
2218void rbp_run_physics(){
2219}
2220void add_physics(struct X3D_Node *node){
2221}
2222void do_CollisionSensorTick(void * ptr){
2223}
2224
2225#endif
2226
2227
2228
2229