void dxJointDBall::updateTargetDistance() { dVector3 p1, p2; if (node[0].body) dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], p1); else dCopyVector3(p1, anchor1); if (node[1].body) dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], p2); else dCopyVector3(p2, anchor2); targetDistance = dCalcPointsDistance3(p1, p2); }
void dxJointDBall::getInfo2( dxJoint::Info2 *info ) { info->erp = erp; info->cfm[0] = cfm; dVector3 globalA1, globalA2; dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1); if (node[1].body) dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2); else dCopyVector3(globalA2, anchor2); dVector3 q; dSubtractVectors3(q, globalA1, globalA2); #ifdef dSINGLE const dReal MIN_LENGTH = REAL(1e-7); #else const dReal MIN_LENGTH = REAL(1e-12); #endif if (dCalcVectorLength3(q) < MIN_LENGTH) { // too small, let's choose an arbitrary direction // heuristic: difference in velocities at anchors dVector3 v1, v2; dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1); if (node[1].body) dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2); else dSetZero(v2, 3); dSubtractVectors3(q, v1, v2); if (dCalcVectorLength3(q) < MIN_LENGTH) { // this direction is as good as any q[0] = 1; q[1] = 0; q[2] = 0; } } dNormalize3(q); info->J1l[0] = q[0]; info->J1l[1] = q[1]; info->J1l[2] = q[2]; dVector3 relA1; dBodyVectorToWorld(node[0].body, anchor1[0], anchor1[1], anchor1[2], relA1); dMatrix3 a1m; dSetZero(a1m, 12); dSetCrossMatrixMinus(a1m, relA1, 4); dMultiply1_331(info->J1a, a1m, q); if (node[1].body) { info->J2l[0] = -q[0]; info->J2l[1] = -q[1]; info->J2l[2] = -q[2]; dVector3 relA2; dBodyVectorToWorld(node[1].body, anchor2[0], anchor2[1], anchor2[2], relA2); dMatrix3 a2m; dSetZero(a2m, 12); dSetCrossMatrixPlus(a2m, relA2, 4); dMultiply1_331(info->J2a, a2m, q); } const dReal k = info->fps * info->erp; info->c[0] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2)); }
void nearCallback(void *, dGeomID a, dGeomID b) { const unsigned max_contacts = 8; dContact contacts[max_contacts]; if (!dGeomGetBody(a) && !dGeomGetBody(b)) return; // don't handle static geom collisions int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact)); //clog << "got " << n << " contacts" << endl; /* Simple contact merging: * If we have contacts that are too close with the same normal, keep only * the one with maximum depth. * The epsilon that defines what "too close" means can be a heuristic. */ int new_n = 0; dReal epsilon = 1e-1; // default /* If we know one of the geoms is a sphere, we can base the epsilon on the * sphere's radius. */ dGeomID s = 0; if ((dGeomGetClass(a) == dSphereClass && (s = a)) || (dGeomGetClass(b) == dSphereClass && (s = b))) { epsilon = dGeomSphereGetRadius(s) * 0.3; } for (int i=0; i<n; ++i) { // this block draws the contact points before merging, in red dMatrix3 r; dRSetIdentity(r); dsSetColor(1, 0, 0); dsSetTexture(DS_NONE); dsDrawSphere(contacts[i].geom.pos, r, 0.008); // let's offset the line a bit to avoid drawing overlap issues float xyzf[3], hprf[3]; dsGetViewpoint(xyzf, hprf); dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])}; dVector3 v; dSubtractVectors3(v, contacts[i].geom.pos, xyz); dVector3 c; dCalcVectorCross3(c, v, contacts[i].geom.pos); dSafeNormalize3(c); dVector3 pos1; dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005); dVector3 pos2; dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05); dsDrawLine(pos1, pos2); // end of contacts drawing code int closest_point = i; for (int j=0; j<new_n; ++j) { dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal); if (alignment > 0.99 // about 8 degrees of difference && dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) { // they are too close closest_point = j; //clog << "found close points: " << j << " and " << i << endl; break; } } if (closest_point != i) { // we discard one of the points if (contacts[i].geom.depth > contacts[closest_point].geom.depth) // the new point is deeper, copy it over closest_point contacts[closest_point] = contacts[i]; } else contacts[new_n++] = contacts[i]; // the point is preserved } //clog << "reduced from " << n << " to " << new_n << endl; n = new_n; for (int i=0; i<n; ++i) { contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP; contacts[i].surface.mu = 10; contacts[i].surface.bounce = 0.2; contacts[i].surface.bounce_vel = 0; contacts[i].surface.soft_erp = 1e-3; //clog << "depth: " << contacts[i].geom.depth << endl; dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]); dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b)); dMatrix3 r; dRSetIdentity(r); dsSetColor(0, 0, 1); dsSetTexture(DS_NONE); dsDrawSphere(contacts[i].geom.pos, r, 0.01); dsSetColor(0, 1, 0); dVector3 pos2; dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1); dsDrawLine(contacts[i].geom.pos, pos2); } //clog << "----" << endl; }
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(); }