void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat)
{
dAASSERT (g && quat);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
CHECK_NOT_LOCKED (g->parent_space);
if (g->offset_posr) {
g->recomputePosr();
// move body such that body+offset = rotation
dxPosR new_final_posr;
dxPosR new_body_posr;
dQtoR (quat, new_final_posr.R);
memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr);
dBodySetRotation(g->body, new_body_posr.R);
dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]);
}
if (g->body) {
// this will call dGeomMoved (g), so we don't have to
dBodySetQuaternion (g->body,quat);
}
else {
dQtoR (quat, g->final_posr->R);
dGeomMoved (g);
}
}
void command (int cmd)
{
// note: 0.0174532925 radians = 1 degree
dQuaternion q;
dMatrix3 m;
switch(cmd)
{
case 'w':
geom1pos[0]+=0.05;
break;
case 'a':
geom1pos[1]-=0.05;
break;
case 's':
geom1pos[0]-=0.05;
break;
case 'd':
geom1pos[1]+=0.05;
break;
case 'e':
geom1pos[2]-=0.05;
break;
case 'q':
geom1pos[2]+=0.05;
break;
case 'i':
dQFromAxisAndAngle (q, 0, 0, 1,0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'j':
dQFromAxisAndAngle (q, 1, 0, 0,0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'k':
dQFromAxisAndAngle (q, 0, 0, 1,-0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'l':
dQFromAxisAndAngle (q, 1, 0, 0,-0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
break;
case 'm':
(drawmode!=DS_POLYFILL)? drawmode=DS_POLYFILL:drawmode=DS_WIREFRAME;
break;
case 'n':
(geoms!=convex)? geoms=convex:geoms=boxes;
break;
default:
dsPrint ("received command %d (`%c')\n",cmd,cmd);
}
#if 0
dGeomSetPosition (geoms[1],
geom1pos[0],
geom1pos[1],
geom1pos[2]);
dQtoR (geom1quat, m);
dGeomSetRotation (geoms[1],m);
#endif
DumpInfo=true;
}
EXPORT_C void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az,
dReal angle)
{
dQuaternion q;
dQFromAxisAndAngle (q,ax,ay,az,angle);
dQtoR (q,R);
}
// converts a osg 4x4 matrix to an ode version of it
void odeRotation( const Pose& pose , dMatrix3& odematrix){
osg::Quat q;
pose.get(q);
// THIS should be
// dQuaternion quat = {q.x(), q.y(), q.z(), q.w() };
dQuaternion quat = {q.w(), q.x(), q.y(), q.z() };
dQtoR(quat, odematrix);
}
void testQuaternionMultiply()
{
HEADER;
dMatrix3 RA,RB,RC,Rtest;
dQuaternion qa,qb,qc;
dReal diff,maxdiff=0;
for (int i=0; i<100; i++) {
makeRandomRotation (RB);
makeRandomRotation (RC);
dRtoQ (RB,qb);
dRtoQ (RC,qc);
dMultiply0 (RA,RB,RC,3,3,3);
dQMultiply0 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply1 (RA,RB,RC,3,3,3);
dQMultiply1 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply2 (RA,RB,RC,3,3,3);
dQMultiply2 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply0 (RA,RC,RB,3,3,3);
transpose3x3 (RA);
dQMultiply3 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
}
printf ("\tmaximum difference = %e - %s\n",maxdiff,
(maxdiff > tol) ? "FAILED" : "passed");
}
void dGeomSetOffsetQuaternion (dxGeom *g, const dQuaternion quat)
{
dAASSERT (g && quat);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
dUASSERT (g->body, "geom must be on a body");
CHECK_NOT_LOCKED (g->parent_space);
if (!g->offset_posr)
{
dGeomCreateOffset (g);
}
dQtoR (quat, g->offset_posr->R);
dGeomMoved (g);
}
void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat)
{
dAASSERT (g && quat);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
CHECK_NOT_LOCKED (g->parent_space);
if (g->body) {
// this will call dGeomMoved (g), so we don't have to
dBodySetQuaternion (g->body,quat);
}
else {
dQtoR (quat, g->R);
dGeomMoved (g);
}
}
void testRtoQandQtoR()
{
HEADER;
dMatrix3 R,I,R2;
dQuaternion q;
int i;
// test makeRandomRotation()
makeRandomRotation (R);
dMultiply2 (I,R,R,3,3,3);
printf ("\tmakeRandomRotation() - %s (1)\n",
cmpIdentityMat3(I) ? "passed" : "FAILED");
// test QtoR() on random normalized quaternions
int ok = 1;
for (i=0; i<100; i++) {
dMakeRandomVector (q,4,1.0);
dNormalize4 (q);
dQtoR (q,R);
dMultiply2 (I,R,R,3,3,3);
if (cmpIdentityMat3(I)==0) ok = 0;
}
printf ("\tQtoR() orthonormality %s (2)\n", ok ? "passed" : "FAILED");
// test R -> Q -> R works
dReal maxdiff=0;
for (i=0; i<100; i++) {
makeRandomRotation (R);
dRtoQ (R,q);
dQtoR (q,R2);
dReal diff = dMaxDifference (R,R2,3,3);
if (diff > maxdiff) maxdiff = diff;
}
printf ("\tmaximum difference = %e - %s (3)\n",maxdiff,
(maxdiff > tol) ? "FAILED" : "passed");
}
void dWorldExportDIF (dWorldID w, FILE *file, const char *prefix)
{
PrintingContext c;
c.file = file;
#if defined(dSINGLE)
c.precision = 7;
#else
c.precision = 15;
#endif
c.indent = 1;
fprintf (file,"-- ODE position and rotation export.\n");
// bodies
int num = 0;
for (dxBody *b=w->firstbody; b; b=(dxBody*)b->next) {
b->tag = num;
fprintf (file,"body %d\n",num);
c.printNum ("",b->posr.pos[0]);
c.printNum ("",b->posr.pos[1]);
c.printNum ("",b->posr.pos[2]);
dMatrix3 R;
dQtoR (b->q, R);
//! Find Roll Pitch and Yaw
// rotation Matrix then convert to Euler Angles
dReal roll = atan2(R[9], R[10]) * 1/DEGTORAD; //phi
dReal pitch = asin(-R[8]) * 1/DEGTORAD; //theta
dReal yaw = atan2(R[4], R[0]) * 1/DEGTORAD; //greek Y
c.printNum ("", roll);
c.printNum ("", pitch);
c.printNum ("", yaw);
num++;
}
fprintf (file,"end of file");
printf ("State.dif Exported.\n");
}
void dGeomSetOffsetWorldQuaternion (dxGeom *g, const dQuaternion quat)
{
dAASSERT (g && quat);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
dUASSERT (g->body, "geom must be on a body");
CHECK_NOT_LOCKED (g->parent_space);
if (!g->offset_posr)
{
dGeomCreateOffset (g);
}
g->recomputePosr();
dxPosR new_final_posr;
memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
dQtoR (quat, new_final_posr.R);
getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr);
dGeomMoved (g);
}
void reset_test()
{
int i;
dMass m,anchor_m;
dReal q[NUM][3], pm[NUM]; // particle positions and masses
dReal pos1[3] = {1,0,1}; // point of reference (POR)
dReal pos2[3] = {-1,0,1}; // point of reference (POR)
// make random particle positions (relative to POR) and masses
for (i=0; i<NUM; i++) {
pm[i] = dRandReal()+0.1;
q[i][0] = dRandReal()-0.5;
q[i][1] = dRandReal()-0.5;
q[i][2] = dRandReal()-0.5;
}
// adjust particle positions so centor of mass = POR
computeMassParams (&m,q,pm);
for (i=0; i<NUM; i++) {
q[i][0] -= m.c[0];
q[i][1] -= m.c[1];
q[i][2] -= m.c[2];
}
if (world) dWorldDestroy (world);
world = dWorldCreate();
anchor_body = dBodyCreate (world);
dBodySetPosition (anchor_body,pos1[0],pos1[1],pos1[2]);
dMassSetBox (&anchor_m,1,SIDE,SIDE,SIDE);
dMassAdjust (&anchor_m,0.1);
dBodySetMass (anchor_body,&anchor_m);
for (i=0; i<NUM; i++) {
particle[i] = dBodyCreate (world);
dBodySetPosition (particle[i],
pos1[0]+q[i][0],pos1[1]+q[i][1],pos1[2]+q[i][2]);
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,pm[i]);
dBodySetMass (particle[i],&m);
}
for (i=0; i < NUM; i++) {
particle_joint[i] = dJointCreateBall (world,0);
dJointAttach (particle_joint[i],anchor_body,particle[i]);
const dReal *p = dBodyGetPosition (particle[i]);
dJointSetBallAnchor (particle_joint[i],p[0],p[1],p[2]);
}
// make test_body with the same mass and inertia of the anchor_body plus
// all the particles
test_body = dBodyCreate (world);
dBodySetPosition (test_body,pos2[0],pos2[1],pos2[2]);
computeMassParams (&m,q,pm);
m.mass += anchor_m.mass;
for (i=0; i<12; i++) m.I[i] = m.I[i] + anchor_m.I[i];
dBodySetMass (test_body,&m);
// rotate the test and anchor bodies by a random amount
dQuaternion qrot;
for (i=0; i<4; i++) qrot[i] = dRandReal()-0.5;
dNormalize4 (qrot);
dBodySetQuaternion (anchor_body,qrot);
dBodySetQuaternion (test_body,qrot);
dMatrix3 R;
dQtoR (qrot,R);
for (i=0; i<NUM; i++) {
dVector3 v;
dMultiply0 (v,R,&q[i][0],3,3,1);
dBodySetPosition (particle[i],pos1[0]+v[0],pos1[1]+v[1],pos1[2]+v[2]);
}
// set random torque
for (i=0; i<3; i++) torque[i] = (dRandReal()-0.5) * 0.1;
iteration=0;
}
void dxStepBody (dxBody *b, dReal h)
{
int j;
#ifdef DEBUG_VALID
dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2]));
#endif
// handle linear velocity
for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dDOT (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
frv[2] = b->finite_rot_axis[2] * k;
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *= REAL(0.5);
dReal theta = k * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = frv[0] * s;
q[2] = frv[1] * s;
q[3] = frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *= REAL(0.5);
dReal theta = wlen * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = b->avel[0] * s;
q[2] = b->avel[1] * s;
q[3] = b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (j=0; j<4; j++) b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (j=0; j<4; j++) b->q[j] += h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (j=0; j<4; j++) b->q[j] += h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->R);
// notify all attached geoms that this body has moved
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
dGeomMoved (geom);
#ifdef DEBUG_VALID
dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2]));
#endif
}
void start()
{
// adjust the starting viewpoint a bit
float xyz[3],hpr[3];
dsGetViewpoint (xyz,hpr);
hpr[0] += 7;
dsSetViewpoint (xyz,hpr);
convex[0]=dCreateConvex (space,
planes,
planecount,
points,
pointcount,
polygons);
convex[1]=dCreateConvex (space,
planes,
planecount,
points,
pointcount,
polygons);
boxes[0]=dCreateBox(space,0.5,0.5,0.5);
boxes[1]=dCreateBox(space,0.5,0.5,0.5);
geoms=convex;
dMatrix3 m1 = { 1,0,0,0,0,1,0,0,0,0,1,0 };
dMatrix3 m2 = { 1,0,0,0,0,1,0,0,0,0,1,0 };
#if 0
dGeomSetPosition (convex[0],
0.0,
0.0,
0.25);
dGeomSetPosition (convex[1],
geom1pos[0],
geom1pos[1],
geom1pos[2]);
dQtoR (geom0quat, m1);
dGeomSetRotation (convex[0],m1);
dQtoR (geom1quat, m2);
dGeomSetRotation (convex[1],m2);
dGeomSetPosition (boxes[0],
0.0,
0.0,
0.25);
dGeomSetPosition (boxes[1],
geom1pos[0],
geom1pos[1],
geom1pos[2]);
dQtoR (geom0quat, m1);
dGeomSetRotation (boxes[0],m1);
dQtoR (geom1quat, m2);
dGeomSetRotation (boxes[1],m2);
#else
dGeomSetPosition (convex[0],
fixed_pos_0[0],
fixed_pos_0[1],
fixed_pos_0[2]);
dGeomSetPosition (convex[1],
fixed_pos_1[0],
fixed_pos_1[1],
fixed_pos_1[2]);
dGeomSetRotation (convex[0],fixed_rot_0);
dGeomSetRotation (convex[1],fixed_rot_1);
dGeomSetPosition (boxes[0],
fixed_pos_0[0],
fixed_pos_0[1],
fixed_pos_0[2]);
dGeomSetPosition (boxes[1],
fixed_pos_1[0],
fixed_pos_1[1],
fixed_pos_1[2]);
dGeomSetRotation (boxes[0],fixed_rot_0);
dGeomSetRotation (boxes[1],fixed_rot_1);
#endif
}
void dWorldExportDIFLong (dWorldID w, FILE *file, const char *prefix)
{
PrintingContext c;
c.file = file;
#if defined(dSINGLE)
c.precision = 7;
#else
c.precision = 15;
#endif
c.indent = 1;
fprintf (file,"-- Dynamics Interchange Format v0.1\n\n%sworld = dynamics.world {\n",prefix);
c.print ("gravity",w->gravity);
c.print ("ODE = {");
c.indent++;
c.print ("ERP",w->global_erp);
c.print ("CFM",w->global_cfm);
c.print ("auto_disable = {");
c.indent++;
c.print ("linear_threshold",w->adis.linear_average_threshold);
c.print ("angular_threshold",w->adis.angular_average_threshold);
c.print ("average_samples",(int)w->adis.average_samples);
c.print ("idle_time",w->adis.idle_time);
c.print ("idle_steps",w->adis.idle_steps);
fprintf (file,"\t\t},\n\t},\n}\n");
c.indent -= 3;
// bodies
int num = 0;
fprintf (file,"%sbody = {}\n",prefix);
for (dxBody *b=w->firstbody; b; b=(dxBody*)b->next) {
b->tag = num;
fprintf (file,"%sbody[%d] = dynamics.body {\n\tworld = %sworld,\n",prefix,num,prefix);
c.indent++;
c.print ("pos",b->posr.pos);
c.print ("q",b->q,4);
dMatrix3 R;
dQtoR (b->q, R);
//! Find Roll Pitch and Yaw
// rotation Matrix then convert to Euler Angles
dReal roll = atan2(R[9], R[10]) * 1/DEGTORAD; //phi
dReal pitch = asin(-R[8]) * 1/DEGTORAD; //theta
dReal yaw = atan2(R[4], R[0]) * 1/DEGTORAD; //greek Y
c.print ("roll ", roll);
c.print ("pitch ", pitch);
c.print ("yaw ", yaw);
c.print ("lvel",b->lvel);
c.print ("avel",b->avel);
c.print ("mass",b->mass.mass);
fprintf (file,"\tI = {{");
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
c.printReal (b->mass.I[i*4+j]);
if (j < 2) fputc (',',file);
}
if (i < 2) fprintf (file,"},{");
}
fprintf (file,"}},\n");
c.printNonzero ("com",b->mass.c);
c.print ("ODE = {");
c.indent++;
if (b->flags & dxBodyFlagFiniteRotation) c.print ("finite_rotation",1);
if (b->flags & dxBodyDisabled) c.print ("disabled",1);
if (b->flags & dxBodyNoGravity) c.print ("no_gravity",1);
if (b->flags & dxBodyAutoDisable) {
c.print ("auto_disable = {");
c.indent++;
c.print ("linear_threshold",b->adis.linear_average_threshold);
c.print ("angular_threshold",b->adis.angular_average_threshold);
c.print ("average_samples",(int)b->adis.average_samples);
c.print ("idle_time",b->adis.idle_time);
c.print ("idle_steps",b->adis.idle_steps);
c.print ("time_left",b->adis_timeleft);
c.print ("steps_left",b->adis_stepsleft);
c.indent--;
c.print ("},");
}
c.printNonzero ("facc",b->facc);
c.printNonzero ("tacc",b->tacc);
if (b->flags & dxBodyFlagFiniteRotationAxis) {
c.print ("finite_rotation_axis",b->finite_rot_axis);
}
c.indent--;
c.print ("},");
if (b->geom) {
c.print ("geometry = {");
c.indent++;
for (dxGeom *g=b->geom; g; g=g->body_next) {
c.print ("{");
c.indent++;
printGeom (c,g);
c.indent--;
c.print ("},");
}
c.indent--;
c.print ("},");
}
c.indent--;
c.print ("}");
num++;
}
// joints
num = 0;
fprintf (file,"%sjoint = {}\n",prefix);
for (dxJoint *j=w->firstjoint; j; j=(dxJoint*)j->next) {
c.indent++;
const char *name = getJointName (j);
fprintf (file,
"%sjoint[%d] = dynamics.%s_joint {\n"
"\tworld = %sworld,\n"
"\tbody = {"
,prefix,num,name,prefix);
if ( j->node[0].body )
fprintf (file,"%sbody[%d]",prefix,j->node[0].body->tag);
if ( j->node[1].body )
fprintf (file,",%sbody[%d]",prefix,j->node[1].body->tag);
fprintf (file,"}\n");
switch (j->type()) {
case dJointTypeBall:
printBall (c,j);
break;
case dJointTypeHinge:
printHinge (c,j);
break;
case dJointTypeSlider:
printSlider (c,j);
break;
case dJointTypeContact:
printContact (c,j);
break;
case dJointTypeUniversal:
printUniversal (c,j);
break;
case dJointTypeHinge2:
printHinge2 (c,j);
break;
case dJointTypeFixed:
printFixed (c,j);
break;
case dJointTypeAMotor:
printAMotor (c,j);
break;
case dJointTypeLMotor:
printLMotor (c,j);
break;
case dJointTypePR:
printPR (c,j);
break;
case dJointTypePU:
printPU (c,j);
break;
case dJointTypePiston:
printPiston (c,j);
break;
default:
c.print("unknown joint");
}
c.indent--;
c.print ("}");
num++;
}
}
void dxStepBody (dxBody *b, dReal h)
{
// cap the angular velocity
if (b->flags & dxBodyMaxAngularSpeed) {
const dReal max_ang_speed = b->max_angular_speed;
const dReal aspeed = dCalcVectorDot3( b->avel, b->avel );
if (aspeed > max_ang_speed*max_ang_speed) {
const dReal coef = max_ang_speed/dSqrt(aspeed);
dScaleVector3(b->avel, coef);
}
}
// end of angular velocity cap
// handle linear velocity
for (unsigned int j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dCalcVectorDot3 (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
frv[2] = b->finite_rot_axis[2] * k;
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *= REAL(0.5);
dReal theta = k * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = frv[0] * s;
q[2] = frv[1] * s;
q[3] = frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *= REAL(0.5);
dReal theta = wlen * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = b->avel[0] * s;
q[2] = b->avel[1] * s;
q[3] = b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (unsigned int j=0; j<4; j++) b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->posr.R);
// notify all attached geoms that this body has moved
dxWorldProcessContext *world_process_context = b->world->UnsafeGetWorldProcessingContext();
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) {
world_process_context->LockForStepbodySerialization();
dGeomMoved (geom);
world_process_context->UnlockForStepbodySerialization();
}
// notify the user
if (b->moved_callback != NULL) {
b->moved_callback(b);
}
// damping
if (b->flags & dxBodyLinearDamping) {
const dReal lin_threshold = b->dampingp.linear_threshold;
const dReal lin_speed = dCalcVectorDot3( b->lvel, b->lvel );
if ( lin_speed > lin_threshold) {
const dReal k = 1 - b->dampingp.linear_scale;
dScaleVector3(b->lvel, k);
}
}
if (b->flags & dxBodyAngularDamping) {
const dReal ang_threshold = b->dampingp.angular_threshold;
const dReal ang_speed = dCalcVectorDot3( b->avel, b->avel );
if ( ang_speed > ang_threshold) {
const dReal k = 1 - b->dampingp.angular_scale;
dScaleVector3(b->avel, k);
}
}
}
void command (int cmd)
{
// note: 0.0174532925 radians = 1 degree
dQuaternion q;
dMatrix3 m;
bool changed = false;
switch(cmd)
{
case 'w':
geom1pos[0]+=0.05;
changed = true;
break;
case 'a':
geom1pos[1]-=0.05;
changed = true;
break;
case 's':
geom1pos[0]-=0.05;
changed = true;
break;
case 'd':
geom1pos[1]+=0.05;
changed = true;
break;
case 'e':
geom1pos[2]-=0.05;
changed = true;
break;
case 'q':
geom1pos[2]+=0.05;
changed = true;
break;
case 'i':
dQFromAxisAndAngle (q, 0, 0, 1,0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
changed = true;
break;
case 'j':
dQFromAxisAndAngle (q, 1, 0, 0,0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
changed = true;
break;
case 'k':
dQFromAxisAndAngle (q, 0, 0, 1,-0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
changed = true;
break;
case 'l':
dQFromAxisAndAngle (q, 1, 0, 0,-0.0174532925);
dQMultiply0(geom1quat,geom1quat,q);
changed = true;
break;
case 'm':
(drawmode!=DS_POLYFILL)? drawmode=DS_POLYFILL:drawmode=DS_WIREFRAME;
break;
case 'n':
(geoms!=convex)? geoms=convex:geoms=boxes;
if(geoms==convex)
{
printf("CONVEX------------------------------------------------------>\n");
}
else
{
printf("BOX--------------------------------------------------------->\n");
}
break;
default:
dsPrint ("received command %d (`%c')\n",cmd,cmd);
}
#if 0
dGeomSetPosition (geoms[1],
geom1pos[0],
geom1pos[1],
geom1pos[2]);
dQtoR (geom1quat, m);
dGeomSetRotation(geoms[1],m);
if(changed)
{
printf("POS: %f,%f,%f\n",geom1pos[0],geom1pos[1],geom1pos[2]);
printf("ROT:\n%f,%f,%f,%f\n%f,%f,%f,%f\n%f,%f,%f,%f\n",
m[0],m[1],m[2],m[3],
m[4],m[5],m[6],m[7],
m[8],m[9],m[10],m[11]);
}
#endif
DumpInfo=true;
}
void createTest()
{
int i,j;
if (world) dWorldDestroy (world);
world = dWorldCreate();
// create random bodies
for (i=0; i<NUM; i++) {
// create bodies at random position and orientation
body[i] = dBodyCreate (world);
// dBodySetPosition (body[i],dRandReal()*2-1,dRandReal()*2-1,
// dRandReal()*2+RADIUS);
dBodySetPosition (body[i],dRandReal()*SIDE-1,dRandReal()*SIDE-1,
dRandReal()*SIDE+RADIUS);
dReal q[4];
for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
dBodySetQuaternion (body[i],q);
// set random velocity
dBodySetLinearVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1);
dBodySetAngularVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1);
// set random mass (random diagonal mass rotated by a random amount)
dMass m;
dMatrix3 R;
dMassSetBox (&m,1,dRandReal()+0.1,dRandReal()+0.1,dRandReal()+0.1);
dMassAdjust (&m,dRandReal()+1);
for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
dQtoR (q,R);
dMassRotate (&m,R);
dBodySetMass (body[i],&m);
}
// create ball-n-socket joints at random positions, linking random bodies
// (but make sure not to link the same pair of bodies twice)
for (i=0; i<NUM*NUM; i++) linked[i] = 0;
for (i=0; i<NUMJ; i++) {
int b1,b2;
do {
b1 = dRandInt (NUM);
b2 = dRandInt (NUM);
} while (linked[b1*NUM + b2] || b1==b2);
linked[b1*NUM + b2] = 1;
linked[b2*NUM + b1] = 1;
joint[i] = dJointCreateBall (world,0);
dJointAttach (joint[i],body[b1],body[b2]);
dJointSetBallAnchor (joint[i],dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*2+RADIUS);
}
for (i=0; i<NUM; i++) {
// move bodies a bit to get some joint error
const dReal *pos = dBodyGetPosition (body[i]);
dBodySetPosition (body[i],pos[0]+dRandReal()*0.2-0.1,
pos[1]+dRandReal()*0.2-0.1,pos[2]+dRandReal()*0.2-0.1);
}
}
// simulation loop
static void simLoop (int pause)
{
static bool todo = false;
if ( todo ) { // DEBUG
static int cnt = 0;
++cnt;
if (cnt == 5)
command ( 'q' );
if (cnt == 10)
dsStop();
}
if (!pause) {
double simstep = 0.01; // 10ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf (dt/simstep);
if (!nrofsteps)
nrofsteps = 1;
for (int i=0; i<nrofsteps && !pause; i++) {
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
update();
dReal radius, length;
dsSetTexture (DS_WOOD);
drawBox (geom[W], 1,1,0);
drawBox (geom[EXT], 0,1,0);
dVector3 anchorPos;
dReal ang1 = 0;
dReal ang2 = 0;
dVector3 axisP, axisR1, axisR2;
if ( dJointTypePU == type ) {
dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
ang1 = pu->getAngle1();
ang2 = pu->getAngle2();
pu->getAxis1 (axisR1);
pu->getAxis2 (axisR2);
pu->getAxisP (axisP);
dJointGetPUAnchor (pu->id(), anchorPos);
}
else if ( dJointTypePR == type ) {
dPRJoint *pr = dynamic_cast<dPRJoint *> (joint);
pr->getAxis1 (axisP);
pr->getAxis2 (axisR1);
dJointGetPRAnchor (pr->id(), anchorPos);
}
// Draw the axisR
if ( geom[INT] ) {
dsSetColor (1,0,1);
dVector3 l;
dGeomBoxGetLengths (geom[INT], l);
const dReal *rotBox = dGeomGetRotation (geom[W]);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i];
dsDrawBox (pos, rotBox, l);
}
dsSetTexture (DS_CHECKERED);
if ( geom[AXIS1] ) {
dQuaternion q, qAng;
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dGeomGetQuaternion (geom[AXIS1], q);
dQuaternion qq;
dQMultiply1 (qq, qAng, q);
dMatrix3 R;
dQtoR (qq,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length);
dsSetColor (1,0,0);
dsDrawCylinder (anchorPos, R, length, radius);
}
if ( dJointTypePU == type && geom[AXIS2] ) {
//dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
dQuaternion q, qAng, qq, qq1;
dGeomGetQuaternion (geom[AXIS2], q);
dQFromAxisAndAngle (qAng, 0, 1, 0, ang2);
dQMultiply1 (qq, qAng, q);
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dQMultiply1 (qq1, qAng, qq);
dMatrix3 R;
dQtoR (qq1,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length);
dsSetColor (0,0,1);
dsDrawCylinder (anchorPos, R, length, radius);
}
dsSetTexture (DS_WOOD);
// Draw the anchor
if ( geom[ANCHOR] ) {
dsSetColor (1,1,1);
dVector3 l;
dGeomBoxGetLengths (geom[ANCHOR], l);
const dReal *rotBox = dGeomGetRotation (geom[D]);
const dReal *posBox = dGeomGetPosition (geom[D]);
dVector3 e;
for (int i=0; i<3; ++i)
e[i] = posBox[i] - anchorPos[i];
dNormalize3 (e);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i];
dsDrawBox (pos, rotBox, l);
}
drawBox (geom[D], 1,1,0);
}
}
static inline void
moveAndRotateBody (dxBody * b, dReal h)
{
int j;
// handle linear velocity
for (j = 0; j < 3; j++)
b->posr.pos[j] += dMUL(h,b->lvel[j]);
if (b->flags & dxBodyFlagFiniteRotation)
{
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis)
{
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dDOT (b->finite_rot_axis, b->avel);
frv[0] = dMUL(b->finite_rot_axis[0],k);
frv[1] = dMUL(b->finite_rot_axis[1],k);
frv[2] = dMUL(b->finite_rot_axis[2],k);
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h = dMUL(h,REAL (0.5));
dReal theta = dMUL(k,h);
q[0] = dCos (theta);
dReal s = dMUL(sinc (theta),h);
q[1] = dMUL(frv[0],s);
q[2] = dMUL(frv[1],s);
q[3] = dMUL(frv[2],s);
}
else
{
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (dMUL(b->avel[0],b->avel[0]) + dMUL(b->avel[1],b->avel[1]) + dMUL(b->avel[2],b->avel[2]));
h = dMUL(h,REAL (0.5));
dReal theta = dMUL(wlen,h);
q[0] = dCos (theta);
dReal s = dMUL(sinc (theta),h);
q[1] = dMUL(b->avel[0],s);
q[2] = dMUL(b->avel[1],s);
q[3] = dMUL(b->avel[2],s);
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2, q, b->q);
for (j = 0; j < 4; j++)
b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis)
{
dReal dq[4];
dWtoDQ (irv, b->q, dq);
for (j = 0; j < 4; j++)
b->q[j] += dMUL(h,dq[j]);
}
}
else
{
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel, b->q, dq);
for (j = 0; j < 4; j++)
b->q[j] += dMUL(h,dq[j]);
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q, b->posr.R);
// notify all attached geoms that this body has moved
for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
dGeomMoved (geom);
}
