int test_dBoxTouchesBox()
{
int k,bt1,bt2;
dVector3 p1,p2,side1,side2;
dMatrix3 R1,R2;
dSimpleSpace space(0);
dGeomID box1 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box1);
dGeomID box2 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box2);
dMakeRandomVector (p1,3,0.5);
dMakeRandomVector (p2,3,0.5);
for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
dGeomSetRotation (box1,R1);
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
dGeomSetRotation (box2,R2);
draw_all_objects (space);
int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2);
int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1);
bt1 = t1 || t2;
bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2);
if (bt1 != bt2) FAILED();
/*
// some more debugging info if necessary
if (bt1 && bt2) printf ("agree - boxes touch\n");
if (!bt1 && !bt2) printf ("agree - boxes don't touch\n");
if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox "
"says no\n");
if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox "
"says yes\n");
*/
PASSED();
}
void dMatrix::makeRandom (dReal range)
{
for (int i=0; i<n; i++) {
for (int j=0; j<m; j++)
data[i*m+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
}
}
static void simLoop (int pause)
{
// stop after a given number of iterations, as long as we are not in
// interactive mode
if (cmd_graphics && !cmd_interactive &&
(iteration >= max_iterations)) {
dsStop();
return;
}
iteration++;
if (!pause) {
// do stuff for this test and check to see if the joint is behaving well
dReal error = doStuffAndGetError (test_num);
if (error > max_error) max_error = error;
if (cmd_interactive && error < dInfinity) {
printf ("scaled error = %.4e\n",error);
}
// take a step
dWorldStep (world,STEPSIZE);
// occasionally re-orient the first body to create a deliberate error.
if (cmd_occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
}
if (cmd_graphics) {
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
if (body[1]) {
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
}
}
static void simLoop (int pause)
{
if (!pause) {
// add random forces and torques to all bodies
int i;
const dReal scale1 = 5;
const dReal scale2 = 5;
for (i=0; i<NUM; i++) {
dBodyAddForce (body[i],
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1));
dBodyAddTorque (body[i],
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1));
}
dWorldStep (world,0.05);
createTest();
}
// float sides[3] = {SIDE,SIDE,SIDE};
dsSetColor (1,1,0);
for (int i=0; i<NUM; i++)
dsDrawSphere (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),RADIUS);
}
int test_plane_point_depth()
{
int j;
dVector3 n,p,q,a,b; // n = plane normal
dReal d;
dSimpleSpace space(0);
dGeomID plane = dCreatePlane (0,0,0,1,0);
dSpaceAdd (space,plane);
// ********** make a random plane
for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
dNormalize3 (n);
d = dRandReal() - 0.5;
dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
dPlaneSpace (n,p,q);
// ********** test point on plane has depth 0
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = 0;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED();
// ********** test arbitrary depth point
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = dRandReal() - 0.5;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol)
FAILED();
// ********** test depth-1 point
a[0] = dRandReal() - 0.5;
a[1] = dRandReal() - 0.5;
a[2] = -1;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED();
PASSED();
}
static void simLoop (int pause)
{
const dReal kd = -0.3; // angular damping constant
const dReal ks = 0.5; // spring constant
if (!pause) {
// add an oscillating torque to body 0, and also damp its rotational motion
static dReal a=0;
const dReal *w = dBodyGetAngularVel (body[0]);
dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
a += 0.01;
// add a spring force to keep the bodies together, otherwise they will
// fly apart along the slider axis.
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
ks*(p2[2]-p1[2]));
dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
ks*(p1[2]-p2[2]));
// occasionally re-orient one of the bodies to create a deliberate error.
if (occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
dWorldStep (world,0.05);
}
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
/*
Test rays within the cylinder
-completely inside
-exiting through side
-exiting through cap
-exiting through corner
Test rays outside the cylinder
*/
int test_ray_and_cylinder()
{
int j;
dContactGeom contact;
dVector3 p,a,b,n;
dMatrix3 R;
dReal r,l,k,x,y;
const int positions = 5;
const int slices = 13;
const int pitch = 11;
dSimpleSpace space(0);
dGeomID ray = dCreateRay(space,4);
dGeomID cyl = dCreateCylinder(space,0.5,1);
// The first thing that happens is the ray is
// rotated into cylinder coordinates. We'll trust that's
// done right. The major axis is in the z-dir.
// Random tests
/*b[0]=4*dRandReal()-2;
b[1]=4*dRandReal()-2;
b[2]=4*dRandReal()-2;
a[0]=2*dRandReal()-1;
a[1]=2*dRandReal()-1;
a[2]=2*dRandReal()-1;*/
// Inside out
b[0]=dRandReal()-0.5;
b[1]=dRandReal()-0.5;
b[2]=dRandReal()-0.5;
a[0]=2*dRandReal()-1;
a[1]=2*dRandReal()-1;
a[2]=2*dRandReal()-1;
// Outside in
/*b[0]=4*dRandReal()-2;
b[1]=4*dRandReal()-2;
b[2]=4*dRandReal()-2;
a[0]=-b[0];
a[1]=-b[1];
a[2]=-b[2];*/
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
// This is just for visual inspection right now.
//if (dCollide (ray,cyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
draw_all_objects (space);
PASSED();
}
int test_sphere_point_depth()
{
int j;
dVector3 p,q;
dMatrix3 R;
dReal r,d;
dSimpleSpace space(0);
dGeomID sphere = dCreateSphere (0,1);
dSpaceAdd (space,sphere);
// ********** make a random sphere of radius r at position p
r = dRandReal()+0.1;
dGeomSphereSetRadius (sphere,r);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (sphere,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (sphere,R);
// ********** test center point has depth r
if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED();
// ********** test point on surface has depth 0
for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = q[j]*r + p[j];
if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED();
// ********** test point at random depth
d = (dRandReal()*2-1) * r;
for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j];
if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED();
PASSED();
}
static void init_test()
{
int i,j;
const dReal scale = 0.5;
// set random boxes
dRandSetSeed (seed);
for (i=0; i < NUM; i++) {
bounds[i][0] = dRandReal()*2-1;
bounds[i][1] = bounds[i][0] + dRandReal()*scale;
bounds[i][2] = dRandReal()*2-1;
bounds[i][3] = bounds[i][2] + dRandReal()*scale;
bounds[i][4] = dRandReal()*2;
bounds[i][5] = bounds[i][4] + dRandReal()*scale;
if (geom[i]) dGeomDestroy (geom[i]);
geom[i] = dCreateBox (space,
bounds[i][1] - bounds[i][0],
bounds[i][3] - bounds[i][2],
bounds[i][5] - bounds[i][4]);
dGeomSetPosition (geom[i],
(bounds[i][0] + bounds[i][1])*0.5,
(bounds[i][2] + bounds[i][3])*0.5,
(bounds[i][4] + bounds[i][5])*0.5);
dGeomSetData (geom[i],(void*)(size_t)(i));
}
// compute all intersections and put the results in "good_matrix"
for (i=0; i < NUM; i++) {
for (j=0; j < NUM; j++) good_matrix[i][j] = 0;
}
for (i=0; i < NUM; i++) hits[i] = 0;
for (i=0; i < NUM; i++) {
for (j=i+1; j < NUM; j++) {
dReal *bounds1 = &bounds[i][0];
dReal *bounds2 = &bounds[j][0];
if (bounds1[0] > bounds2[1] ||
bounds1[1] < bounds2[0] ||
bounds1[2] > bounds2[3] ||
bounds1[3] < bounds2[2] ||
bounds1[4] > bounds2[5] ||
bounds1[5] < bounds2[4]) continue;
good_matrix[i][j] = 1;
good_matrix[j][i] = 1;
hits[i]++;
hits[j]++;
}
}
}
dReal doStuffAndGetError (int n)
{
switch (n) {
// ********** fixed joint
case 0: { // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
// check the orientations are the same
const dReal *R1 = dBodyGetRotation (body[0]);
const dReal *R2 = dBodyGetRotation (body[1]);
dReal err1 = dMaxDifference (R1,R2,3,3);
// check the body offset is correct
dVector3 p,pp;
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
dMULTIPLY1_331 (pp,R1,p);
pp[0] += 0.5;
pp[1] += 0.5;
return (err1 + length (pp)) * 300;
}
case 1: { // 1 body to static env
addOscillatingTorque (0.1);
// check the orientation is the identity
dReal err1 = cmpIdentity (dBodyGetRotation (body[0]));
// check the body offset is correct
dVector3 p;
const dReal *p1 = dBodyGetPosition (body[0]);
for (int i=0; i<3; i++) p[i] = p1[i];
p[0] -= 0.25;
p[1] -= 0.25;
p[2] -= 1;
return (err1 + length (p)) * 1e6;
}
case 2: { // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
// check the body offset is correct
// Should really check body rotation too. Oh well.
const dReal *R1 = dBodyGetRotation (body[0]);
dVector3 p,pp;
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
dMULTIPLY1_331 (pp,R1,p);
pp[0] += 0.5;
pp[1] += 0.5;
return length(pp) * 300;
}
case 3: { // 1 body to static env with relative rotation
addOscillatingTorque (0.1);
// check the body offset is correct
dVector3 p;
const dReal *p1 = dBodyGetPosition (body[0]);
for (int i=0; i<3; i++) p[i] = p1[i];
p[0] -= 0.25;
p[1] -= 0.25;
p[2] -= 1;
return length (p) * 1e6;
}
// ********** hinge joint
case 200: // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
return dInfinity;
case 220: // hinge angle polarity test
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
if (iteration == 40) {
dReal a = dJointGetHingeAngle (joint);
if (a > 0.5 && a < 1) return 0; else return 10;
}
return 0;
case 221: { // hinge angle rate test
static dReal last_angle = 0;
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
dReal a = dJointGetHingeAngle (joint);
dReal r = dJointGetHingeAngleRate (joint);
dReal er = (a-last_angle)/STEPSIZE; // estimated rate
last_angle = a;
return fabs(r-er) * 4e4;
}
case 230: // hinge motor rate (and polarity) test
case 231: { // ...with stops
static dReal a = 0;
dReal r = dJointGetHingeAngleRate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHingeParam (joint,dParamVel,cos(a));
if (n==231) return dInfinity;
return err * 1e6;
}
// ********** slider joint
case 300: // 2 body
addOscillatingTorque (0.05);
dampRotationalMotion (0.1);
addSpringForce (0.5);
return dInfinity;
case 320: // slider angle polarity test
dBodyAddForce (body[0],0,0,0.1);
dBodyAddForce (body[1],0,0,-0.1);
if (iteration == 40) {
dReal a = dJointGetSliderPosition (joint);
if (a > 0.2 && a < 0.5) return 0; else return 10;
return a;
}
return 0;
case 321: { // slider angle rate test
static dReal last_pos = 0;
dBodyAddForce (body[0],0,0,0.1);
dBodyAddForce (body[1],0,0,-0.1);
dReal p = dJointGetSliderPosition (joint);
dReal r = dJointGetSliderPositionRate (joint);
dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact)
last_pos = p;
return fabs(r-er) * 1e9;
}
case 330: // slider motor rate (and polarity) test
case 331: { // ...with stops
static dReal a = 0;
dReal r = dJointGetSliderPositionRate (joint);
dReal err = fabs (0.7*cos(a) - r);
if (a < 0.04) err = 0;
a += 0.03;
dJointSetSliderParam (joint,dParamVel,0.7*cos(a));
if (n==331) return dInfinity;
return err * 1e6;
}
// ********** hinge-2 joint
case 420: // hinge-2 steering angle polarity test
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
if (iteration == 40) {
dReal a = dJointGetHinge2Angle1 (joint);
if (a > 0.5 && a < 0.6) return 0; else return 10;
}
return 0;
case 421: { // hinge-2 steering angle rate test
static dReal last_angle = 0;
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
dReal a = dJointGetHinge2Angle1 (joint);
dReal r = dJointGetHinge2Angle1Rate (joint);
dReal er = (a-last_angle)/STEPSIZE; // estimated rate
last_angle = a;
return fabs(r-er)*2e4;
}
case 430: // hinge 2 steering motor rate (+polarity) test
case 431: { // ...with stops
static dReal a = 0;
dReal r = dJointGetHinge2Angle1Rate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHinge2Param (joint,dParamVel,cos(a));
if (n==431) return dInfinity;
return err * 1e6;
}
case 432: { // hinge 2 wheel motor rate (+polarity) test
static dReal a = 0;
dReal r = dJointGetHinge2Angle2Rate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHinge2Param (joint,dParamVel2,cos(a));
return err * 1e6;
}
// ********** angular motor joint
case 600: { // test euler angle calculations
// desired euler angles from last iteration
static dReal a1,a2,a3;
// find actual euler angles
dReal aa1 = dJointGetAMotorAngle (joint,0);
dReal aa2 = dJointGetAMotorAngle (joint,1);
dReal aa3 = dJointGetAMotorAngle (joint,2);
// printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3);
dReal err = dInfinity;
if (iteration > 0) {
err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3);
err *= 1e10;
}
// get random base rotation for both bodies
dMatrix3 Rbase;
dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5),
3*(dRandReal()-0.5), 3*(dRandReal()-0.5));
dBodySetRotation (body[0],Rbase);
// rotate body 2 by random euler angles w.r.t. body 1
a1 = 3.14 * 2 * (dRandReal()-0.5);
a2 = 1.57 * 2 * (dRandReal()-0.5);
a3 = 3.14 * 2 * (dRandReal()-0.5);
dMatrix3 R1,R2,R3,Rtmp1,Rtmp2;
dRFromAxisAndAngle (R1,0,0,1,-a1);
dRFromAxisAndAngle (R2,0,1,0,a2);
dRFromAxisAndAngle (R3,1,0,0,-a3);
dMultiply0 (Rtmp1,R2,R3,3,3,3);
dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3);
dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3);
dBodySetRotation (body[1],Rtmp1);
// printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);
return err;
}
// ********** universal joint
case 700: { // 2 body: joint constraint
dVector3 ax1, ax2;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
return fabs(10*dDOT(ax1, ax2));
}
case 701: { // 2 body: angle 1 rate
static dReal last_angle = 0;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
// I'm not sure why the error is so large here.
return fabs(r - er) * 1e1;
}
case 702: { // 2 body: angle 2 rate
static dReal last_angle = 0;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
// I'm not sure why the error is so large here.
return fabs(r - er) * 1e1;
}
case 720: { // universal transmit torque test: constraint error
dVector3 ax1, ax2;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
return fabs(10*dDOT(ax1, ax2));
}
case 721: { // universal transmit torque test: angle1 rate
static dReal last_angle = 0;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 722: { // universal transmit torque test: angle2 rate
static dReal last_angle = 0;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 730:{
dVector3 ax1, ax2;
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
return fabs(10*dDOT(ax1, ax2));
}
case 731:{
dVector3 ax1;
static dReal last_angle = 0;
dJointGetUniversalAxis1(joint, ax1);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 2e3;
}
case 732:{
dVector3 ax1;
static dReal last_angle = 0;
dJointGetUniversalAxis1(joint, ax1);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 740:{
dVector3 ax1, ax2;
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
return fabs(10*dDOT(ax1, ax2));
}
case 741:{
dVector3 ax2;
static dReal last_angle = 0;
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 742:{
dVector3 ax2;
static dReal last_angle = 0;
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e4;
}
}
return dInfinity;
}
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;
}
static void command (int cmd)
{
size_t i;
int k;
dReal sides[3];
dMass m;
int setBody;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'y')
{
setBody = 0;
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
if (obj[i].body) {
dBodyDestroy (obj[i].body);
}
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) {
dGeomDestroy (obj[i].geom[k]);
}
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos)
{
dBodySetPosition (obj[i].body,
dRandReal()*2-1 + platpos[0],
dRandReal()*2-1 + platpos[1],
dRandReal()+2 + platpos[2]);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else
{
dBodySetPosition (obj[i].body,
platpos[0],
platpos[1],
platpos[2]+2);
dRSetIdentity (R);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*) i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
else if (cmd == 'y') {
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
if (!setBody)
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) {
dGeomSetBody (obj[i].geom[k],obj[i].body);
}
}
dBodySetMass (obj[i].body,&m);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == '1') {
write_world = 1;
}
else if (cmd == ' ') {
mov_time = 0;
}
else if (cmd == 'm') {
mov_type = mov_type==1 ? 2 : 1;
mov_time = 0;
}
}
int test_dBoxBox()
{
int k,bt;
dVector3 p1,p2,side1,side2,normal,normal2;
dMatrix3 R1,R2;
dReal depth,depth2;
int code;
dContactGeom contact[48];
dSimpleSpace space(0);
dGeomID box1 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box1);
dGeomID box2 = dCreateBox (0,1,1,1);
dSpaceAdd (space,box2);
dMakeRandomVector (p1,3,0.5);
dMakeRandomVector (p2,3,0.5);
for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
// dRSetIdentity (R1); // we can also try this
// dRSetIdentity (R2);
dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
dGeomSetRotation (box1,R1);
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
dGeomSetRotation (box2,R2);
code = 0;
depth = 0;
bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal,&depth,&code,8,contact,
sizeof(dContactGeom));
if (bt==1) {
p2[0] += normal[0] * 0.96 * depth;
p2[1] += normal[1] * 0.96 * depth;
p2[2] += normal[2] * 0.96 * depth;
bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
sizeof(dContactGeom));
/*
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
draw_all_objects (space);
*/
if (bt != 1) {
FAILED();
dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
draw_all_objects (space);
}
p2[0] += normal[0] * 0.08 * depth;
p2[1] += normal[1] * 0.08 * depth;
p2[2] += normal[2] * 0.08 * depth;
bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
sizeof(dContactGeom));
if (bt != 0) FAILED();
// dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
// draw_all_objects (space);
}
// printf ("code=%2d depth=%.4f ",code,depth);
PASSED();
}
extern "C" ODE_API int dTestSolveLCP()
{
const int n = 100;
size_t memreq = EstimateTestSolveLCPMemoryReq(n);
dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, NULL, NULL);
if (arena == NULL) {
return 0;
}
int i,nskip = dPAD(n);
#ifdef dDOUBLE
const dReal tol = REAL(1e-9);
#endif
#ifdef dSINGLE
const dReal tol = REAL(1e-4);
#endif
printf ("dTestSolveLCP()\n");
dReal *A = arena->AllocateArray<dReal> (n*nskip);
dReal *x = arena->AllocateArray<dReal> (n);
dReal *b = arena->AllocateArray<dReal> (n);
dReal *w = arena->AllocateArray<dReal> (n);
dReal *lo = arena->AllocateArray<dReal> (n);
dReal *hi = arena->AllocateArray<dReal> (n);
dReal *A2 = arena->AllocateArray<dReal> (n*nskip);
dReal *b2 = arena->AllocateArray<dReal> (n);
dReal *lo2 = arena->AllocateArray<dReal> (n);
dReal *hi2 = arena->AllocateArray<dReal> (n);
dReal *tmp1 = arena->AllocateArray<dReal> (n);
dReal *tmp2 = arena->AllocateArray<dReal> (n);
double total_time = 0;
for (int count=0; count < 1000; count++) {
BEGIN_STATE_SAVE(arena, saveInner) {
// form (A,b) = a random positive definite LCP problem
dMakeRandomMatrix (A2,n,n,1.0);
dMultiply2 (A,A2,A2,n,n,n);
dMakeRandomMatrix (x,n,1,1.0);
dMultiply0 (b,A,x,n,n,1);
for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
// choose `nub' in the range 0..n-1
int nub = 50; //dRandInt (n);
// make limits
for (i=0; i<nub; i++) lo[i] = -dInfinity;
for (i=0; i<nub; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = 0;
//for (i=nub; i<n; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = -dInfinity;
//for (i=nub; i<n; i++) hi[i] = 0;
for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01);
// set a few limits to lo=hi=0
/*
for (i=0; i<10; i++) {
int j = dRandInt (n-nub) + nub;
lo[j] = 0;
hi[j] = 0;
}
*/
// solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
// SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
// ensure that it doesn't get referenced (if it does, the answer will be
// wrong).
memcpy (A2,A,n*nskip*sizeof(dReal));
dClearUpperTriangle (A2,n);
memcpy (b2,b,n*sizeof(dReal));
memcpy (lo2,lo,n*sizeof(dReal));
memcpy (hi2,hi,n*sizeof(dReal));
dSetZero (x,n);
dSetZero (w,n);
dStopwatch sw;
dStopwatchReset (&sw);
dStopwatchStart (&sw);
dSolveLCP (arena,n,A2,x,b2,w,nub,lo2,hi2,0);
dStopwatchStop (&sw);
double time = dStopwatchTime(&sw);
total_time += time;
double average = total_time / double(count+1) * 1000.0;
// check the solution
dMultiply0 (tmp1,A,x,n,n,1);
for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
dReal diff = dMaxDifference (tmp1,tmp2,n,1);
// printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
// diff > tol ? "FAILED" : "passed");
if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
int n1=0,n2=0,n3=0;
for (i=0; i<n; i++) {
if (x[i]==lo[i] && w[i] >= 0) {
n1++; // ok
}
else if (x[i]==hi[i] && w[i] <= 0) {
n2++; // ok
}
else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
n3++; // ok
}
else {
dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
x[i],w[i],lo[i],hi[i]);
}
}
// pacifier
printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3);
printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average);
} END_STATE_SAVE(arena, saveInner);
}
extern "C" ODE_API int dTestSolveLCP()
{
const int n = 100;
//size_t memreq = EstimateTestSolveLCPMemoryReq(n);
//dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, nullptr, nullptr);
//if (arena == nullptr) {
// return 0;
//}
//int i,nskip = dPAD(n);
int i;
int nskip = n;
#ifdef dDOUBLE
const dReal tol = REAL(1e-9);
#endif
#ifdef dSINGLE
const dReal tol = REAL(1e-4);
#endif
printf ("dTestSolveLCP()\n");
dReal *A = new dReal[n*nskip];
dReal *x = new dReal[n];
dReal *b = new dReal[n];
dReal *w = new dReal[n];
dReal *lo = new dReal[n];
dReal *hi = new dReal[n];
dReal *A2 = new dReal[n*nskip];
dReal *b2 = new dReal[n];
dReal *lo2 = new dReal[n];
dReal *hi2 = new dReal[n];
dReal *tmp1 = new dReal[n];
dReal *tmp2 = new dReal[n];
// double total_time = 0;
for (int count=0; count < 1000; count++) {
// form (A,b) = a random positive definite LCP problem
dMakeRandomMatrix (A2,n,n,1.0);
dMultiply2 (A,A2,A2,n,n,n);
dMakeRandomMatrix (x,n,1,1.0);
dMultiply0 (b,A,x,n,n,1);
for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
// choose `nub' in the range 0..n-1
int nub = 50; //dRandInt (n);
// make limits
for (i=0; i<nub; i++) lo[i] = -dInfinity;
for (i=0; i<nub; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = 0;
//for (i=nub; i<n; i++) hi[i] = dInfinity;
//for (i=nub; i<n; i++) lo[i] = -dInfinity;
//for (i=nub; i<n; i++) hi[i] = 0;
for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01);
// set a few limits to lo=hi=0
/*
for (i=0; i<10; i++) {
int j = dRandInt (n-nub) + nub;
lo[j] = 0;
hi[j] = 0;
}
*/
// solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
// SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
// ensure that it doesn't get referenced (if it does, the answer will be
// wrong).
memcpy (A2,A,n*nskip*sizeof(dReal));
dClearUpperTriangle (A2,n);
memcpy (b2,b,n*sizeof(dReal));
memcpy (lo2,lo,n*sizeof(dReal));
memcpy (hi2,hi,n*sizeof(dReal));
dSetZero (x,n);
dSetZero (w,n);
dSolveLCP (n,A2,x,b2,w,nub,lo2,hi2,0);
// check the solution
dMultiply0 (tmp1,A,x,n,n,1);
for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
dReal diff = dMaxDifference (tmp1,tmp2,n,1);
// printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
// diff > tol ? "FAILED" : "passed");
if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
int n1=0,n2=0,n3=0;
for (i=0; i<n; i++) {
if (x[i]==lo[i] && w[i] >= 0) {
n1++; // ok
}
else if (x[i]==hi[i] && w[i] <= 0) {
n2++; // ok
}
else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
n3++; // ok
}
else {
dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
x[i],w[i],lo[i],hi[i]);
}
}
// pacifier
printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3);
}
delete[] A;
delete[] x;
delete[] b;
delete[] w;
delete[] lo ;
delete[] hi ;
delete[] A2 ;
delete[] b2 ;
delete[] lo2;
delete[] hi2;
delete[] tmp1;
delete[] tmp2;
return 1;
}
int test_ray_and_plane()
{
int j;
dContactGeom contact;
dVector3 n,p,q,a,b,g,h; // n,d = plane parameters
dMatrix3 R;
dReal d;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID plane = dCreatePlane (0,0,0,1,0);
dSpaceAdd (space,ray);
dSpaceAdd (space,plane);
// ********** make a random plane
for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
dNormalize3 (n);
d = dRandReal() - 0.5;
dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
dPlaneSpace (n,p,q);
// ********** test finite length ray below plane
dGeomRaySetLength (ray,0.09);
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = -dRandReal()*0.5 - 0.1;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
dGeomSetPosition (ray,b[0],b[1],b[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray above plane
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = dRandReal()*0.5 + 0.01;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
g[0] = dRandReal()-0.5;
g[1] = dRandReal()-0.5;
g[2] = dRandReal() + 0.01;
for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
dNormalize3 (h);
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray that intersects plane
a[0] = dRandReal()-0.5;
a[1] = dRandReal()-0.5;
a[2] = dRandReal()-0.5;
for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
g[0] = dRandReal()-0.5;
g[1] = dRandReal()-0.5;
g[2] = dRandReal()-0.5;
for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
dNormalize3 (h);
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom))) {
// test that contact is on plane surface
if (dFabs (dCalcVectorDot3(contact.pos,n) - d) > tol) FAILED();
// also check normal signs
if (dCalcVectorDot3 (h,contact.normal) > 0) FAILED();
// also check contact point depth
if (dFabs (dGeomPlanePointDepth
(plane,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
draw_all_objects (space);
}
// ********** test ray that just misses
for (j=0; j<3; j++) b[j] = (1+d)*n[j];
for (j=0; j<3; j++) h[j] = -n[j];
dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
dGeomRaySetLength (ray,0.99);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray that just hits
dGeomRaySetLength (ray,1.01);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// ********** test polarity with typical ground plane
dGeomPlaneSetParams (plane,0,0,1,0);
for (j=0; j<3; j++) a[j] = 0.1;
for (j=0; j<3; j++) b[j] = 0;
a[2] = 1;
b[2] = -1;
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
if (dFabs (contact.depth - 1) > tol) FAILED();
a[2] = -1;
b[2] = 1;
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
if (dFabs (contact.depth - 1) > tol) FAILED();
PASSED();
}
int test_ray_and_ccylinder()
{
int j;
dContactGeom contact;
dVector3 p,a,b,n;
dMatrix3 R;
dReal r,l,k,x,y;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID ccyl = dCreateCapsule (0,1,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,ccyl);
// ********** make a random capped cylinder
r = dRandReal()*0.5 + 0.01;
l = dRandReal()*1 + 0.01;
dGeomCapsuleSetParams (ccyl,r,l);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (ccyl,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ccyl,R);
// ********** test ray completely within ccyl
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) a[j] = p[j] + r*0.99*a[j] + k*0.99*R[j*4+2];
for (j=0; j<3; j++) b[j] = dRandReal()-0.5;
dNormalize3 (b);
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) b[j] = p[j] + r*0.99*b[j] + k*0.99*R[j*4+2];
dGeomRaySetLength (ray,dCalcPointsDistance3(a,b));
for (j=0; j<3; j++) b[j] -= a[j];
dNormalize3 (b);
dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just misses (between caps)
k = dRandReal()*2*M_PI;
x = sin(k);
y = cos(k);
for (j=0; j<3; j++) a[j] = x*R[j*4+0] + y*R[j*4+1];
k = (dRandReal()-0.5)*l;
for (j=0; j<3; j++) b[j] = -a[j]*r*2 + k*R[j*4+2] + p[j];
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
dGeomRaySetLength (ray,r*0.99);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just hits (between caps)
dGeomRaySetLength (ray,r*1.01);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// ********** test ray outside ccyl that just misses (caps)
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
if (dCalcVectorDot3_14(a,R+2) < 0) {
for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r + l*0.5*R[j*4+2];
}
else {
for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r - l*0.5*R[j*4+2];
}
dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
dGeomRaySetLength (ray,r*0.99);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray outside ccyl that just hits (caps)
dGeomRaySetLength (ray,r*1.01);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// ********** test random rays
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
for (j=0; j<3; j++) n[j] = dRandReal()-0.5;
dNormalize3 (n);
dGeomRaySet (ray,a[0],a[1],a[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom))) {
// check depth of contact point
if (dFabs (dGeomCapsulePointDepth
(ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// check normal signs
if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED();
draw_all_objects (space);
}
PASSED();
}
int test_ray_and_box()
{
int i,j;
dContactGeom contact;
dVector3 s,p,q,n,q2,q3,q4; // s = box sides
dMatrix3 R;
dReal k;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID box = dCreateBox (0,1,1,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,box);
// ********** make a random box
for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
dGeomBoxSetLengths (box,s[0],s[1],s[2]);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (box,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (box,R);
// ********** test zero length ray just inside box
dGeomRaySetLength (ray,0);
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 0.99*0.5*s[i]; else q[i] = -0.99*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test zero length ray just outside box
dGeomRaySetLength (ray,0);
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally contained inside the box
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*0.99*s[j];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
for (j=0; j<3; j++) q3[j] = (dRandReal()-0.5)*0.99*s[j];
dMultiply0 (q4,dGeomGetRotation(box),q3,3,3,1);
for (j=0; j<3; j++) q4[j] += p[j];
for (j=0; j<3; j++) n[j] = q4[j] - q2[j];
dNormalize3 (n);
dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,dCalcPointsDistance3(q2,q4));
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally outside the box
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q3[j] = q2[j] + p[j];
dNormalize3 (q2);
dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just above surface
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q3[j] = 2*q2[j] + p[j];
k = dSqrt(q2[0]*q2[0] + q2[1]*q2[1] + q2[2]*q2[2]);
for (j=0; j<3; j++) q2[j] = -q2[j];
dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
dGeomRaySetLength (ray,k*0.99);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just below surface
dGeomRaySetLength (ray,k*1.01);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
// ********** test contact point position for random rays
for (j=0; j<3; j++) q[j] = dRandReal()*s[j];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
for (j=0; j<3; j++) q3[j] = dRandReal()-0.5;
dNormalize3 (q3);
dGeomRaySet (ray,q2[0],q2[1],q2[2],q3[0],q3[1],q3[2]);
dGeomRaySetLength (ray,10);
if (dCollide (ray,box,1,&contact,sizeof(dContactGeom))) {
// check depth of contact point
if (dFabs (dGeomBoxPointDepth
(box,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
// check position of contact point
for (j=0; j<3; j++) contact.pos[j] -= p[j];
dMultiply1 (q,dGeomGetRotation(box),contact.pos,3,3,1);
if ( dFabs(dFabs (q[0]) - 0.5*s[0]) > tol &&
dFabs(dFabs (q[1]) - 0.5*s[1]) > tol &&
dFabs(dFabs (q[2]) - 0.5*s[2]) > tol) {
FAILED();
}
// also check normal signs
if (dCalcVectorDot3 (q3,contact.normal) > 0) FAILED();
draw_all_objects (space);
}
PASSED();
}
int test_ray_and_sphere()
{
int j;
dContactGeom contact;
dVector3 p,q,q2,n,v1;
dMatrix3 R;
dReal r,k;
dSimpleSpace space(0);
dGeomID ray = dCreateRay (0,0);
dGeomID sphere = dCreateSphere (0,1);
dSpaceAdd (space,ray);
dSpaceAdd (space,sphere);
// ********** make a random sphere of radius r at position p
r = dRandReal()+0.1;
dGeomSphereSetRadius (sphere,r);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (sphere,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (sphere,R);
// ********** test zero length ray just inside sphere
dGeomRaySetLength (ray,0);
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
dGeomSetPosition (ray,q[0],q[1],q[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test zero length ray just outside that sphere
dGeomRaySetLength (ray,0);
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
dGeomSetPosition (ray,q[0],q[1],q[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ray,R);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally contained inside the sphere
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
k = dRandReal();
for (j=0; j<3; j++) q[j] = k*r*0.99 * q[j] + p[j];
dMakeRandomVector (q2,3,1.0);
dNormalize3 (q2);
k = dRandReal();
for (j=0; j<3; j++) q2[j] = k*r*0.99 * q2[j] + p[j];
for (j=0; j<3; j++) n[j] = q2[j] - q[j];
dNormalize3 (n);
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,dCalcPointsDistance3(q,q2));
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test finite length ray totally outside the sphere
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
do {
dMakeRandomVector (n,3,1.0);
dNormalize3 (n);
}
while (dCalcVectorDot3(n,q) < 0); // make sure normal goes away from sphere
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,100);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just above surface
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
for (j=0; j<3; j++) n[j] = -q[j];
for (j=0; j<3; j++) q2[j] = 2*r * q[j] + p[j];
dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,0.99*r);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test ray from outside to just below surface
dGeomRaySetLength (ray,1.01*r);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
for (j=0; j<3; j++) q2[j] = r * q[j] + p[j];
if (dCalcPointsDistance3 (contact.pos,q2) > tol) FAILED();
// ********** test contact point distance for random rays
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
k = dRandReal()+0.5;
for (j=0; j<3; j++) q[j] = k*r * q[j] + p[j];
dMakeRandomVector (n,3,1.0);
dNormalize3 (n);
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,100);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom))) {
k = dCalcPointsDistance3 (contact.pos,dGeomGetPosition(sphere));
if (dFabs(k - r) > tol) FAILED();
// also check normal signs
if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED();
// also check depth of contact point
if (dFabs (dGeomSpherePointDepth
(sphere,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
FAILED();
draw_all_objects (space);
}
// ********** test tangential grazing - miss
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
dPlaneSpace (q,n,v1);
for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
for (j=0; j<3; j++) q[j] -= n[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
// ********** test tangential grazing - hit
dMakeRandomVector (q,3,1.0);
dNormalize3 (q);
dPlaneSpace (q,n,v1);
for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
for (j=0; j<3; j++) q[j] -= n[j];
dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
dGeomRaySetLength (ray,2);
if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
PASSED();
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = "../../drawstuff/textures";
if(argc==2)
{
fn.path_to_textures = argv[1];
}
// create world
world = dWorldCreate();
space = dHashSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5); //-0.5
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
for (int i=0;i<TERRAINNODES*TERRAINNODES;i++) pTerrainHeights[i] = vTerrainHeight * dRandReal();
terrainY = dCreateTerrainY(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);
terrainZ = dCreateTerrainZ(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);
dMatrix3 R;
dRFromZAxis(R, 0.2f, 0.2f, 0.2f);
dGeomSetPosition(terrainY,0.f,0.f,0.5f);
dGeomSetRotation(terrainY,R);
dGeomSetPosition(terrainZ,0.f,0.f,0.5f);
dGeomSetRotation(terrainZ,R);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
return 0;
}
static void command (int cmd)
{
size_t i;
int j,k;
dReal sides[3];
dMass m;
int setBody;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v')
{
setBody = 0;
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos)
{
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else
{
dReal maxheight = 0;
for (k=0; k<num; k++)
{
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRSetIdentity (R);
//dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*) i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
//<---- Convex Object
else if (cmd == 'v')
{
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
#if 0
obj[i].geom[0] = dCreateConvex (space,
planes,
planecount,
points,
pointcount,
polygons);
#else
obj[i].geom[0] = dCreateConvex (space,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons);
#endif
}
//----> Convex Object
else if (cmd == 'y') {
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x' && USE_GEOM_OFFSET) {
setBody = 1;
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
obj[i].geom[k] = dCreateBox (space,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule (space,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle (drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate (&m2,drot[k]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd (&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody (obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition (obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass (obj[i].body,&m);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCapsule (0,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// Translation *after* rotation
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<GPB; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
if (!setBody)
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == '1') {
write_world = 1;
}
else if (cmd == 'p'&& selected >= 0)
{
const dReal* pos = dGeomGetPosition(obj[selected].geom[0]);
const dReal* rot = dGeomGetRotation(obj[selected].geom[0]);
printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]);
printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n",
rot[0],rot[1],rot[2],rot[3],
rot[4],rot[5],rot[6],rot[7],
rot[8],rot[9],rot[10],rot[11]);
}
else if (cmd == 'f' && selected >= 0 && selected < num) {
if (dBodyIsEnabled(obj[selected].body))
doFeedback = 1;
}
}
int test_box_point_depth()
{
int i,j;
dVector3 s,p,q,q2; // s = box sides
dMatrix3 R;
dReal ss,d; // ss = smallest side
dSimpleSpace space(0);
dGeomID box = dCreateBox (0,1,1,1);
dSpaceAdd (space,box);
// ********** make a random box
for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
dGeomBoxSetLengths (box,s[0],s[1],s[2]);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (box,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (box,R);
// ********** test center point has depth of smallest side
ss = 1e9;
for (j=0; j<3; j++) if (s[j] < ss) ss = s[j];
if (dFabs(dGeomBoxPointDepth (box,p[0],p[1],p[2]) - 0.5*ss) > tol)
FAILED();
// ********** test point on surface has depth 0
for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
i = dRandInt (3);
if (dRandReal() > 0.5) q[i] = 0.5*s[i]; else q[i] = -0.5*s[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2])) > tol) FAILED();
// ********** test points outside box have -ve depth
for (j=0; j<3; j++) {
q[j] = 0.5*s[j] + dRandReal() + 0.01;
if (dRandReal() > 0.5) q[j] = -q[j];
}
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) >= 0) FAILED();
// ********** test points inside box have +ve depth
for (j=0; j<3; j++) q[j] = s[j] * 0.99 * (dRandReal()-0.5);
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) <= 0) FAILED();
// ********** test random depth of point aligned along axis (up to ss deep)
i = dRandInt (3);
for (j=0; j<3; j++) q[j] = 0;
d = (dRandReal()*(ss*0.5+1)-1);
q[i] = s[i]*0.5 - d;
if (dRandReal() > 0.5) q[i] = -q[i];
dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
for (j=0; j<3; j++) q2[j] += p[j];
if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) - d) >= tol) FAILED();
PASSED();
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' || cmd == 'v') {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
} else if (cmd == 'v') {
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
obj[i].geom[0] = dCreateConvex(space,
planes,
planecount,
points,
pointcount,
polygons);
}
else if (cmd == 'y') {
sides[1] *= 0.5;
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'm') {
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount,
(dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL);
obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
// remember the mesh's dTriMeshDataID on its userdata for convenience.
dGeomSetData(obj[i].geom[0], new_tmdata);
dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
}
else if (cmd == 'x') {
setBody = true;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
} else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
int test_ccylinder_point_depth()
{
int j;
dVector3 p,a;
dMatrix3 R;
dReal r,l,beta,x,y,d;
dSimpleSpace space(0);
dGeomID ccyl = dCreateCapsule (0,1,1);
dSpaceAdd (space,ccyl);
// ********** make a random ccyl
r = dRandReal()*0.5 + 0.01;
l = dRandReal()*1 + 0.01;
dGeomCapsuleSetParams (ccyl,r,l);
dMakeRandomVector (p,3,1.0);
dGeomSetPosition (ccyl,p[0],p[1],p[2]);
dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*10-5);
dGeomSetRotation (ccyl,R);
// ********** test point on axis has depth of 'radius'
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + l*beta*R[j*4+2];
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - r) >= tol)
FAILED();
// ********** test point on surface (excluding caps) has depth 0
beta = dRandReal()*2*M_PI;
x = r*sin(beta);
y = r*cos(beta);
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
// ********** test point on surface of caps has depth 0
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
if (dCalcVectorDot3_14(a,R+2) > 0) {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*r + l*0.5*R[j*4+2];
}
else {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*r - l*0.5*R[j*4+2];
}
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
// ********** test point inside ccyl has positive depth
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + a[j]*r*0.99 + l*beta*R[j*4+2];
if (dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) < 0) FAILED();
// ********** test point depth (1)
d = (dRandReal()*2-1) * r;
beta = dRandReal()*2*M_PI;
x = (r-d)*sin(beta);
y = (r-d)*cos(beta);
beta = dRandReal()-0.5;
for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
FAILED();
// ********** test point depth (2)
d = (dRandReal()*2-1) * r;
for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
dNormalize3 (a);
if (dCalcVectorDot3_14(a,R+2) > 0) {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) + l*0.5*R[j*4+2];
}
else {
for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) - l*0.5*R[j*4+2];
}
if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
FAILED();
PASSED();
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v'
/* || cmd == 'l' */) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
/*
// cylinder option not yet implemented
else if (cmd == 'l') {
sides[1] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
*/
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x') {
setBody = true;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
} else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
} else if (cmd == 'v') {
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
obj[i].geom[0] = dCreateConvex(space,
planes,
planecount,
points,
pointcount,
polygons);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
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);
}
}
static void command (int cmd)
{
size_t i;
int j,k;
dReal sides[3];
dMass m;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
/* || cmd == 'l' */) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*) i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
}
/*
// cylinder option not yet implemented
else if (cmd == 'l') {
sides[1] *= 0.5;
dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
*/
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCCylinder (0,radius,length);
dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<2; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == '1') {
write_world = 1;
}
}
static void createMesh()
{
int i,k;
dReal sides[3];
dMass m;
loading = 1;
Sleep(100);
i = num;
num++;
obj[i].body = dBodyCreate (ode_world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
cMesh* new_object = new cMesh(world);
new_object->loadFromFile("resources\\models\\bunny.obj");
new_object->computeGlobalPositions();
new_object->createAABBCollisionDetector(true, true);
new_object->computeAllNormals();
// set material properties of object
cMaterial new_material;
new_material.setStiffness(20.0);
new_material.setDynamicFriction(0.2);
new_material.setStaticFriction(0.4);
new_material.m_ambient.set(0.3, 0.3, 0.8);
new_material.m_diffuse.set(0.8, 0.8, 0.8);
new_material.m_specular.set(1.0, 1.0, 1.0);
new_material.setShininess(100);
new_object->useMaterial(true, true);
new_object->setMaterial(new_material, true);
new_object->useColors(false, true);
cODEProxy* new_proxy = new cODEProxy(default_proxy);
new_proxy->enableDynamicProxy(true);
cursor->setRenderingMode(RENDER_DEVICE);
new_proxy->m_defaultObject = new_object;
new_proxy->initialize(world, cursor->m_deviceGlobalPos);
cursor->m_pointForceAlgos.push_back(new_proxy);
VertexCount = new_object->getNumVertices(true);
Vertices = new float[VertexCount*3];
IndexCount = 3*new_object->getNumTriangles(true);
Indices = new int[IndexCount];
// This will hold all the parents we're still searching...
std::list<cMesh*> meshes_to_descend;
meshes_to_descend.push_front(new_object);
// While there are still parent meshes to process
int cnt = 0;
int cnt2 = 0;
while(meshes_to_descend.empty() == 0) {
// Grab the next parent
cMesh* cur_mesh = meshes_to_descend.front();
meshes_to_descend.pop_front();
// Put all his children on the list of parents to process
for(unsigned int i=0; i<cur_mesh->getNumChildren(); i++) {
cGenericObject* cur_object = cur_mesh->getChild(i);
// Only process cMesh children
cMesh* cur_mesh = dynamic_cast<cMesh*>(cur_object);
if (cur_mesh)
{
unsigned int i;
for (i=0; i<cur_mesh->getNumVertices(false); i++)
{
Vertices[cnt] = cur_mesh->getVertex(i)->getPos().x;
Vertices[cnt+1] = cur_mesh->getVertex(i)->getPos().y;
Vertices[cnt+2] = cur_mesh->getVertex(i)->getPos().z;
cnt+=3;
}
for (i=0; i<cur_mesh->getNumTriangles(false); i++)
{
Indices[cnt2] = cur_mesh->getTriangle(i)->getIndexVertex0();
Indices[cnt2+1] = cur_mesh->getTriangle(i)->getIndexVertex1();
Indices[cnt2+2] = cur_mesh->getTriangle(i)->getIndexVertex2();
cnt2+=3;
}
meshes_to_descend.push_back(cur_mesh);
}
}
}
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, Indices, IndexCount, 3 * sizeof(int));
obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
// remember the mesh's dTriMeshDataID on its userdata for convenience.
dGeomSetData(obj[i].geom[0], new_tmdata);
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
for (k=0; k < GPB; k++)
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
dBodySetMass (obj[i].body,&m);
new_object->setPos(0,0,100);
new_object->computeGlobalPositions(1);
objects.push_back(new_object);
syncPoses();
world->addChild(new_object);
loading = 0;
Sleep(100);
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' ) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
else if (cmd == 'y') {
sides[1] *= 0.5;
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'm') {
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount,
(dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
// remember the mesh's dTriMeshDataID on its userdata for convenience.
dGeomSetData(obj[i].geom[0], new_tmdata);
dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCapsule (0,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<2; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
void testMassFunctions()
{
dMass m;
int i,j;
dReal q[NUMP][3]; // particle positions
dReal pm[NUMP]; // particle masses
dMass m1,m2;
dMatrix3 R;
HEADER;
printf ("\t");
dMassSetZero (&m);
TRAP_MESSAGE (dMassSetParameters (&m,10, 0,0,0, 1,2,3, 4,5,6),
printf (" FAILED (1)\n"), printf (" passed (1)\n"));
printf ("\t");
dMassSetZero (&m);
TRAP_MESSAGE (dMassSetParameters (&m,10, 0.1,0.2,0.15, 3,5,14, 3.1,3.2,4),
printf ("passed (2)\n") , printf (" FAILED (2)\n"));
if (m.mass==10 && m.c[0]==REAL(0.1) && m.c[1]==REAL(0.2) &&
m.c[2]==REAL(0.15) && m._I(0,0)==3 && m._I(1,1)==5 && m._I(2,2)==14 &&
m._I(0,1)==REAL(3.1) && m._I(0,2)==REAL(3.2) && m._I(1,2)==4 &&
m._I(1,0)==REAL(3.1) && m._I(2,0)==REAL(3.2) && m._I(2,1)==4)
printf ("\tpassed (3)\n"); else printf ("\tFAILED (3)\n");
dMassSetZero (&m);
dMassSetSphere (&m,1.4, 0.86);
if (cmp(m.mass,3.73002719949386) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
cmp(m._I(0,0),1.10349124669826) &&
cmp(m._I(1,1),1.10349124669826) &&
cmp(m._I(2,2),1.10349124669826) &&
m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
printf ("\tpassed (4)\n"); else printf ("\tFAILED (4)\n");
dMassSetZero (&m);
dMassSetCapsule (&m,1.3,1,0.76,1.53);
if (cmp(m.mass,5.99961928996029) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
cmp(m._I(0,0),1.59461986077384) &&
cmp(m._I(1,1),4.21878433864904) &&
cmp(m._I(2,2),4.21878433864904) &&
m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
printf ("\tpassed (5)\n"); else printf ("\tFAILED (5)\n");
dMassSetZero (&m);
dMassSetBox (&m,0.27,3,4,5);
if (cmp(m.mass,16.2) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
cmp(m._I(0,0),55.35) && cmp(m._I(1,1),45.9) && cmp(m._I(2,2),33.75) &&
m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
printf ("\tpassed (6)\n"); else printf ("\tFAILED (6)\n");
// test dMassAdjust?
// make random particles and compute the mass, COM and inertia, then
// translate and repeat.
for (i=0; i<NUMP; i++) {
pm[i] = dRandReal()+0.5;
for (j=0; j<3; j++) {
q[i][j] = 2.0*(dRandReal()-0.5);
}
}
computeMassParams (&m1,q,pm);
memcpy (&m2,&m1,sizeof(dMass));
dMassTranslate (&m2,1,2,-3);
for (i=0; i<NUMP; i++) {
q[i][0] += 1;
q[i][1] += 2;
q[i][2] -= 3;
}
computeMassParams (&m1,q,pm);
compareMassParams (&m1,&m2,"7");
// rotate the masses
_R(0,0) = -0.87919618797635;
_R(0,1) = 0.15278881840384;
_R(0,2) = -0.45129772879842;
_R(1,0) = -0.47307856232664;
_R(1,1) = -0.39258064912909;
_R(1,2) = 0.78871864932708;
_R(2,0) = -0.05666336483842;
_R(2,1) = 0.90693771059546;
_R(2,2) = 0.41743652473765;
dMassRotate (&m2,R);
for (i=0; i<NUMP; i++) {
dReal a[3];
dMultiply0 (a,&_R(0,0),&q[i][0],3,3,1);
q[i][0] = a[0];
q[i][1] = a[1];
q[i][2] = a[2];
}
computeMassParams (&m1,q,pm);
compareMassParams (&m1,&m2,"8");
}
