// Ripped from Opcode 1.1. static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, float& u, float& v){ //calculate plane of triangle dVector4 Plane; dCROSS(Plane, =, Edge0, Edge1); Plane[3] = dDOT(Plane, Origin); //normalize dNormalize4(Plane); /* If the center of the sphere is within the positive halfspace of the * triangle's plane, allow a contact to be generated. * If the center of the sphere made it into the positive halfspace of a * back-facing triangle, then the physics update and/or velocity needs * to be adjusted (penetration has occured anyway). */ float side = dDOT(Plane,Center) - Plane[3]; if(side < 0.0f) { return false; } // now onto the bulk of the collision... dVector3 Diff; Diff[0] = Origin[0] - Center[0]; Diff[1] = Origin[1] - Center[1]; Diff[2] = Origin[2] - Center[2]; Diff[3] = Origin[3] - Center[3]; float A00 = dDOT(Edge0, Edge0); float A01 = dDOT(Edge0, Edge1); float A11 = dDOT(Edge1, Edge1); float B0 = dDOT(Diff, Edge0); float B1 = dDOT(Diff, Edge1); float C = dDOT(Diff, Diff); float Det = dFabs(A00 * A11 - A01 * A01); u = A01 * B1 - A11 * B0; v = A01 * B0 - A00 * B1; float DistSq; if (u + v <= Det){ if(u < REAL(0.0)){ if(v < REAL(0.0)){ // region 4 if(B0 < REAL(0.0)){ v = REAL(0.0); if (-B0 >= A00){ u = REAL(1.0); DistSq = A00 + REAL(2.0) * B0 + C; } else{ u = -B0 / A00; DistSq = B0 * u + C; } } else{ u = REAL(0.0); if(B1 >= REAL(0.0)){ v = REAL(0.0); DistSq = C; } else if(-B1 >= A11){ v = REAL(1.0); DistSq = A11 + REAL(2.0) * B1 + C; } else{ v = -B1 / A11; DistSq = B1 * v + C; } } } else{ // region 3 u = REAL(0.0); if(B1 >= REAL(0.0)){ v = REAL(0.0); DistSq = C; } else if(-B1 >= A11){ v = REAL(1.0); DistSq = A11 + REAL(2.0) * B1 + C; } else{ v = -B1 / A11; DistSq = B1 * v + C; } } } else if(v < REAL(0.0)){ // region 5 v = REAL(0.0); if (B0 >= REAL(0.0)){ u = REAL(0.0); DistSq = C; } else if (-B0 >= A00){ u = REAL(1.0); DistSq = A00 + REAL(2.0) * B0 + C; } else{ u = -B0 / A00; DistSq = B0 * u + C; } } else{ // region 0 // minimum at interior point if (Det == REAL(0.0)){ u = REAL(0.0); v = REAL(0.0); DistSq = FLT_MAX; } else{ float InvDet = REAL(1.0) / Det; u *= InvDet; v *= InvDet; DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; } } } else{ float Tmp0, Tmp1, Numer, Denom; if(u < REAL(0.0)){ // region 2 Tmp0 = A01 + B0; Tmp1 = A11 + B1; if (Tmp1 > Tmp0){ Numer = Tmp1 - Tmp0; Denom = A00 - REAL(2.0) * A01 + A11; if (Numer >= Denom){ u = REAL(1.0); v = REAL(0.0); DistSq = A00 + REAL(2.0) * B0 + C; } else{ u = Numer / Denom; v = REAL(1.0) - u; DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; } } else{ u = REAL(0.0); if(Tmp1 <= REAL(0.0)){ v = REAL(1.0); DistSq = A11 + REAL(2.0) * B1 + C; } else if(B1 >= REAL(0.0)){ v = REAL(0.0); DistSq = C; } else{ v = -B1 / A11; DistSq = B1 * v + C; } } } else if(v < REAL(0.0)){ // region 6 Tmp0 = A01 + B1; Tmp1 = A00 + B0; if (Tmp1 > Tmp0){ Numer = Tmp1 - Tmp0; Denom = A00 - REAL(2.0) * A01 + A11; if (Numer >= Denom){ v = REAL(1.0); u = REAL(0.0); DistSq = A11 + REAL(2.0) * B1 + C; } else{ v = Numer / Denom; u = REAL(1.0) - v; DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; } } else{ v = REAL(0.0); if (Tmp1 <= REAL(0.0)){ u = REAL(1.0); DistSq = A00 + REAL(2.0) * B0 + C; } else if(B0 >= REAL(0.0)){ u = REAL(0.0); DistSq = C; } else{ u = -B0 / A00; DistSq = B0 * u + C; } } } else{ // region 1 Numer = A11 + B1 - A01 - B0; if (Numer <= REAL(0.0)){ u = REAL(0.0); v = REAL(1.0); DistSq = A11 + REAL(2.0) * B1 + C; } else{ Denom = A00 - REAL(2.0) * A01 + A11; if (Numer >= Denom){ u = REAL(1.0); v = REAL(0.0); DistSq = A00 + REAL(2.0) * B0 + C; } else{ u = Numer / Denom; v = REAL(1.0) - u; DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; } } } } Dist = dSqrt(dFabs(DistSq)); if (Dist <= Radius){ Dist = Radius - Dist; return true; } else return false; }
int dcTriListCollider::CollideBox(dxGeom* Box, int Flags, dContactGeom* Contacts, int Stride) { Fvector AABB; dVector3 BoxSides; dGeomBoxGetLengths(Box,BoxSides); dReal* R=const_cast<dReal*>(dGeomGetRotation(Box)); AABB.x=(dFabs(BoxSides[0]*R[0])+dFabs(BoxSides[1]*R[1])+dFabs(BoxSides[2]*R[2]))/2.f +10.f*EPS_L; AABB.y=(dFabs(BoxSides[0]*R[4])+dFabs(BoxSides[1]*R[5])+dFabs(BoxSides[2]*R[6]))/2.f +10.f*EPS_L; AABB.z=(dFabs(BoxSides[0]*R[8])+dFabs(BoxSides[1]*R[9])+dFabs(BoxSides[2]*R[10]))/2.f+10.f*EPS_L; dBodyID box_body=dGeomGetBody(Box); if(box_body) { const dReal*velocity=dBodyGetLinearVel(box_body); AABB.x+=dFabs(velocity[0])*0.04f; AABB.y+=dFabs(velocity[1])*0.04f; AABB.z+=dFabs(velocity[2])*0.04f; } BoxTri bt(*this); return dSortTriPrimitiveCollide (bt, Box, Geometry, Flags, Contacts, Stride, AABB ); }
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; }
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(); }
int dBoxBox (const dVector3 p1, const dMatrix3 R1, const dVector3 side1, const dVector3 p2, const dMatrix3 R2, const dVector3 side2, dVector3 normal, dReal *depth, int *return_code, int flags, dContactGeom *contact, int skip) { const dReal fudge_factor = REAL(1.05); dVector3 p,pp,normalC={0,0,0}; const dReal *normalR = 0; dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val; int i,j,invert_normal,code; // get vector from centers of box 1 to box 2, relative to box 1 p[0] = p2[0] - p1[0]; p[1] = p2[1] - p1[1]; p[2] = p2[2] - p1[2]; dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 // get side lengths / 2 A[0] = side1[0]*REAL(0.5); A[1] = side1[1]*REAL(0.5); A[2] = side1[2]*REAL(0.5); B[0] = side2[0]*REAL(0.5); B[1] = side2[1]*REAL(0.5); B[2] = side2[2]*REAL(0.5); // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13); Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23); Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33); // for all 15 possible separating axes: // * see if the axis separates the boxes. if so, return 0. // * find the depth of the penetration along the separating axis (s2) // * if this is the largest depth so far, record it. // the normal vector will be set to the separating axis with the smallest // depth. note: normalR is set to point to a column of R1 or R2 if that is // the smallest depth normal so far. otherwise normalR is 0 and normalC is // set to a vector relative to body 1. invert_normal is 1 if the sign of // the normal should be flipped. do { #define TST(expr1,expr2,norm,cc) \ expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \ s2 = dFabs(expr1_val) - (expr2); \ if (s2 > 0) return 0; \ if (s2 > s) { \ s = s2; \ normalR = norm; \ invert_normal = ((expr1_val) < 0); \ code = (cc); \ if (flags & CONTACTS_UNIMPORTANT) break; \ } s = -dInfinity; invert_normal = 0; code = 0; // separating axis = u1,u2,u3 TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); // separating axis = v1,v2,v3 TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); // note: cross product axes need to be scaled when s is computed. // normal (n1,n2,n3) is relative to box 1. #undef TST #define TST(expr1,expr2,n1,n2,n3,cc) \ expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \ s2 = dFabs(expr1_val) - (expr2); \ if (s2 > 0) return 0; \ l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ if (l > 0) { \ s2 /= l; \ if (s2*fudge_factor > s) { \ s = s2; \ normalR = 0; \ normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ invert_normal = ((expr1_val) < 0); \ code = (cc); \ if (flags & CONTACTS_UNIMPORTANT) break; \ } \ } // We only need to check 3 edges per box // since parallel edges are equivalent. // separating axis = u1 x (v1,v2,v3) TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); // separating axis = u2 x (v1,v2,v3) TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); // separating axis = u3 x (v1,v2,v3) TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); #undef TST } while (0); if (!code) return 0; // if we get to this point, the boxes interpenetrate. compute the normal // in global coordinates. if (normalR) { normal[0] = normalR[0]; normal[1] = normalR[4]; normal[2] = normalR[8]; } else { dMULTIPLY0_331 (normal,R1,normalC); } if (invert_normal) { normal[0] = -normal[0]; normal[1] = -normal[1]; normal[2] = -normal[2]; } *depth = -s; // compute contact point(s) if (code > 6) { // An edge from box 1 touches an edge from box 2. // find a point pa on the intersecting edge of box 1 dVector3 pa; dReal sign; // Copy p1 into pa for (i=0; i<3; i++) pa[i] = p1[i]; // why no memcpy? // Get world position of p2 into pa for (j=0; j<3; j++) { sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0); for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; } // find a point pb on the intersecting edge of box 2 dVector3 pb; // Copy p2 into pb for (i=0; i<3; i++) pb[i] = p2[i]; // why no memcpy? // Get world position of p2 into pb for (j=0; j<3; j++) { sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0); for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; } dReal alpha,beta; dVector3 ua,ub; // Get direction of first edge for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; // Get direction of second edge for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; // Get closest points between edges (one at each) dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); for (i=0; i<3; i++) pa[i] += ua[i]*alpha; for (i=0; i<3; i++) pb[i] += ub[i]*beta; // Set the contact point as halfway between the 2 closest points for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]); contact[0].depth = *depth; *return_code = code; return 1; } // okay, we have a face-something intersection (because the separating // axis is perpendicular to a face). define face 'a' to be the reference // face (i.e. the normal vector is perpendicular to this) and face 'b' to be // the incident face (the closest face of the other box). // Note: Unmodified parameter values are being used here const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb; if (code <= 3) { // One of the faces of box 1 is the reference face Ra = R1; // Rotation of 'a' Rb = R2; // Rotation of 'b' pa = p1; // Center (location) of 'a' pb = p2; // Center (location) of 'b' Sa = A; // Side Lenght of 'a' Sb = B; // Side Lenght of 'b' } else { // One of the faces of box 2 is the reference face Ra = R2; // Rotation of 'a' Rb = R1; // Rotation of 'b' pa = p2; // Center (location) of 'a' pb = p1; // Center (location) of 'b' Sa = B; // Side Lenght of 'a' Sb = A; // Side Lenght of 'b' } // nr = normal vector of reference face dotted with axes of incident box. // anr = absolute values of nr. /* The normal is flipped if necessary so it always points outward from box 'a', box 'b' is thus always the incident box */ dVector3 normal2,nr,anr; if (code <= 3) { normal2[0] = normal[0]; normal2[1] = normal[1]; normal2[2] = normal[2]; } else { normal2[0] = -normal[0]; normal2[1] = -normal[1]; normal2[2] = -normal[2]; } // Rotate normal2 in incident box opposite direction dMULTIPLY1_331 (nr,Rb,normal2); anr[0] = dFabs (nr[0]); anr[1] = dFabs (nr[1]); anr[2] = dFabs (nr[2]); // find the largest compontent of anr: this corresponds to the normal // for the incident face. the other axis numbers of the incident face // are stored in a1,a2. int lanr,a1,a2; if (anr[1] > anr[0]) { if (anr[1] > anr[2]) { a1 = 0; lanr = 1; a2 = 2; } else { a1 = 0; a2 = 1; lanr = 2; } } else { if (anr[0] > anr[2]) { lanr = 0; a1 = 1; a2 = 2; } else { a1 = 0; a2 = 1; lanr = 2; } } // compute center point of incident face, in reference-face coordinates dVector3 center; if (nr[lanr] < 0) { for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; } else { for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; } // find the normal and non-normal axis numbers of the reference box int codeN,code1,code2; if (code <= 3) codeN = code-1; else codeN = code-4; if (codeN==0) { code1 = 1; code2 = 2; } else if (codeN==1) { code1 = 0; code2 = 2; } else { code1 = 0; code2 = 1; } // find the four corners of the incident face, in reference-face coordinates dReal quad[8]; // 2D coordinate of incident face (x,y pairs) dReal c1,c2,m11,m12,m21,m22; c1 = dDOT14 (center,Ra+code1); c2 = dDOT14 (center,Ra+code2); // optimize this? - we have already computed this data above, but it is not // stored in an easy-to-index format. for now it's quicker just to recompute // the four dot products. m11 = dDOT44 (Ra+code1,Rb+a1); m12 = dDOT44 (Ra+code1,Rb+a2); m21 = dDOT44 (Ra+code2,Rb+a1); m22 = dDOT44 (Ra+code2,Rb+a2); { dReal k1 = m11*Sb[a1]; dReal k2 = m21*Sb[a1]; dReal k3 = m12*Sb[a2]; dReal k4 = m22*Sb[a2]; quad[0] = c1 - k1 - k3; quad[1] = c2 - k2 - k4; quad[2] = c1 - k1 + k3; quad[3] = c2 - k2 + k4; quad[4] = c1 + k1 + k3; quad[5] = c2 + k2 + k4; quad[6] = c1 + k1 - k3; quad[7] = c2 + k2 - k4; } // find the size of the reference face dReal rect[2]; rect[0] = Sa[code1]; rect[1] = Sa[code2]; // intersect the incident and reference faces dReal ret[16]; int n = intersectRectQuad (rect,quad,ret); if (n < 1) return 0; // this should never happen // convert the intersection points into reference-face coordinates, // and compute the contact position and depth for each point. only keep // those points that have a positive (penetrating) depth. delete points in // the 'ret' array as necessary so that 'point' and 'ret' correspond. dReal point[3*8]; // penetrating contact points dReal dep[8]; // depths for those points dReal det1 = dRecip(m11*m22 - m12*m21); m11 *= det1; m12 *= det1; m21 *= det1; m22 *= det1; int cnum = 0; // number of penetrating contact points found for (j=0; j < n; j++) { dReal k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); for (i=0; i<3; i++) point[cnum*3+i] = center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); if (dep[cnum] >= 0) { ret[cnum*2] = ret[j*2]; ret[cnum*2+1] = ret[j*2+1]; cnum++; if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } } if (cnum < 1) { return 0; // this should not happen, yet does at times (demo_plane2d single precision). } // we can't generate more contacts than we actually have int maxc = flags & NUMC_MASK; if (maxc > cnum) maxc = cnum; if (maxc < 1) maxc = 1; // Even though max count must not be zero this check is kept for backward compatibility as this is a public function if (cnum <= maxc) { // we have less contacts than we need, so we use them all for (j=0; j < cnum; j++) { dContactGeom *con = CONTACT(contact,skip*j); for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; con->depth = dep[j]; } } else { dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed // we have more contacts than are wanted, some of them must be culled. // find the deepest point, it is always the first contact. int i1 = 0; dReal maxdepth = dep[0]; for (i=1; i<cnum; i++) { if (dep[i] > maxdepth) { maxdepth = dep[i]; i1 = i; } } int iret[8]; cullPoints (cnum,ret,maxc,i1,iret); for (j=0; j < maxc; j++) { dContactGeom *con = CONTACT(contact,skip*j); for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; con->depth = dep[iret[j]]; } cnum = maxc; } *return_code = code; return cnum; }
void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, dReal *outer_w/*=NULL*/, int nub, dReal *lo, dReal *hi, int *findex) { dAASSERT (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n); # ifndef dNODEBUG { // check restrictions on lo and hi for (int k=0; k<n; ++k) dIASSERT (lo[k] <= 0 && hi[k] >= 0); } # endif // if all the variables are unbounded then we can just factor, solve, // and return if (nub >= n) { dReal *d = new dReal[n]; dSetZero (d, n); int nskip = dPAD(n); dFactorLDLT (A, d, n, nskip); dSolveLDLT (A, d, b, n, nskip); memcpy (x, b, n*sizeof(dReal)); return; } const int nskip = dPAD(n); dReal *L = new dReal[ (n*nskip)]; dReal *d = new dReal[ (n)]; dReal *w = outer_w ? outer_w : (new dReal[n]); dReal *delta_w = new dReal[ (n)]; dReal *delta_x = new dReal[ (n)]; dReal *Dell = new dReal[ (n)]; dReal *ell = new dReal[ (n)]; #ifdef ROWPTRS dReal **Arows = new dReal* [n]; #else dReal **Arows = NULL; #endif int *p = new int[n]; int *C = new int[n]; // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i) bool *state = new bool[n]; // create LCP object. note that tmp is set to delta_w to save space, this // optimization relies on knowledge of how tmp is used, so be careful! dLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows); int adj_nub = lcp.getNub(); // loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the // LCP conditions then i is added to the appropriate index set. otherwise // x(i),w(i) is driven either +ve or -ve to force it to the valid region. // as we drive x(i), x(C) is also adjusted to keep w(C) at zero. // while driving x(i) we maintain the LCP conditions on the other variables // 0..i-1. we do this by watching out for other x(i),w(i) values going // outside the valid region, and then switching them between index sets // when that happens. bool hit_first_friction_index = false; for (int i=adj_nub; i<n; ++i) { bool s_error = false; // the index i is the driving index and indexes i+1..n-1 are "dont care", // i.e. when we make changes to the system those x's will be zero and we // don't care what happens to those w's. in other words, we only consider // an (i+1)*(i+1) sub-problem of A*x=b+w. // if we've hit the first friction index, we have to compute the lo and // hi values based on the values of x already computed. we have been // permuting the indexes, so the values stored in the findex vector are // no longer valid. thus we have to temporarily unpermute the x vector. // for the purposes of this computation, 0*infinity = 0 ... so if the // contact constraint's normal force is 0, there should be no tangential // force applied. if (!hit_first_friction_index && findex && findex[i] >= 0) { // un-permute x into delta_w, which is not being used at the moment for (int j=0; j<n; ++j) delta_w[p[j]] = x[j]; // set lo and hi values for (int k=i; k<n; ++k) { dReal wfk = delta_w[findex[k]]; if (wfk == 0) { hi[k] = 0; lo[k] = 0; } else { hi[k] = dFabs (hi[k] * wfk); lo[k] = -hi[k]; } } hit_first_friction_index = true; } // thus far we have not even been computing the w values for indexes // greater than i, so compute w[i] now. w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i]; // if lo=hi=0 (which can happen for tangential friction when normals are // 0) then the index will be assigned to set N with some state. however, // set C's line has zero size, so the index will always remain in set N. // with the "normal" switching logic, if w changed sign then the index // would have to switch to set C and then back to set N with an inverted // state. this is pointless, and also computationally expensive. to // prevent this from happening, we use the rule that indexes with lo=hi=0 // will never be checked for set changes. this means that the state for // these indexes may be incorrect, but that doesn't matter. // see if x(i),w(i) is in a valid region if (lo[i]==0 && w[i] >= 0) { lcp.transfer_i_to_N (i); state[i] = false; } else if (hi[i]==0 && w[i] <= 0) { lcp.transfer_i_to_N (i); state[i] = true; } else if (w[i]==0) { // this is a degenerate case. by the time we get to this test we know // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve, // and similarly that hi > 0. this means that the line segment // corresponding to set C is at least finite in extent, and we are on it. // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C() lcp.solve1 (delta_x,i,0,1); lcp.transfer_i_to_C (i); } else { // we must push x(i) and w(i) for (;;) { int dir; dReal dirf; // find direction to push on x(i) if (w[i] <= 0) { dir = 1; dirf = REAL(1.0); } else { dir = -1; dirf = REAL(-1.0); } // compute: delta_x(C) = -dir*A(C,C)\A(C,i) lcp.solve1 (delta_x,i,dir); // note that delta_x[i] = dirf, but we wont bother to set it // compute: delta_w = A*delta_x ... note we only care about // delta_w(N) and delta_w(i), the rest is ignored lcp.pN_equals_ANC_times_qC (delta_w,delta_x); lcp.pN_plusequals_ANi (delta_w,i,dir); delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i)*dirf; // find largest step we can take (size=s), either to drive x(i),w(i) // to the valid LCP region or to drive an already-valid variable // outside the valid region. int cmd = 1; // index switching command int si = 0; // si = index to switch if cmd>3 dReal s = -w[i]/delta_w[i]; if (dir > 0) { if (hi[i] < dInfinity) { dReal s2 = (hi[i]-x[i])*dirf; // was (hi[i]-x[i])/dirf // step to x(i)=hi(i) if (s2 < s) { s = s2; cmd = 3; } } } else { if (lo[i] > -dInfinity) { dReal s2 = (lo[i]-x[i])*dirf; // was (lo[i]-x[i])/dirf // step to x(i)=lo(i) if (s2 < s) { s = s2; cmd = 2; } } } { const int numN = lcp.numN(); for (int k=0; k < numN; ++k) { const int indexN_k = lcp.indexN(k); if (!state[indexN_k] ? delta_w[indexN_k] < 0 : delta_w[indexN_k] > 0) { // don't bother checking if lo=hi=0 if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue; dReal s2 = -w[indexN_k] / delta_w[indexN_k]; if (s2 < s) { s = s2; cmd = 4; si = indexN_k; } } } } { const int numC = lcp.numC(); for (int k=adj_nub; k < numC; ++k) { const int indexC_k = lcp.indexC(k); if (delta_x[indexC_k] < 0 && lo[indexC_k] > -dInfinity) { dReal s2 = (lo[indexC_k]-x[indexC_k]) / delta_x[indexC_k]; if (s2 < s) { s = s2; cmd = 5; si = indexC_k; } } if (delta_x[indexC_k] > 0 && hi[indexC_k] < dInfinity) { dReal s2 = (hi[indexC_k]-x[indexC_k]) / delta_x[indexC_k]; if (s2 < s) { s = s2; cmd = 6; si = indexC_k; } } } } //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C", // "C->NL","C->NH"}; //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i); // if s <= 0 then we've got a problem. if we just keep going then // we're going to get stuck in an infinite loop. instead, just cross // our fingers and exit with the current solution. if (s <= REAL(0.0)) { dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",(double)s); if (i < n) { dSetZero (x+i,n-i); dSetZero (w+i,n-i); } s_error = true; break; } // apply x = x + s * delta_x lcp.pC_plusequals_s_times_qC (x, s, delta_x); x[i] += s * dirf; // apply w = w + s * delta_w lcp.pN_plusequals_s_times_qN (w, s, delta_w); w[i] += s * delta_w[i]; // void *tmpbuf; // switch indexes between sets if necessary switch (cmd) { case 1: // done w[i] = 0; lcp.transfer_i_to_C (i); break; case 2: // done x[i] = lo[i]; state[i] = false; lcp.transfer_i_to_N (i); break; case 3: // done x[i] = hi[i]; state[i] = true; lcp.transfer_i_to_N (i); break; case 4: // keep going w[si] = 0; lcp.transfer_i_from_N_to_C (si); break; case 5: // keep going x[si] = lo[si]; state[si] = false; lcp.transfer_i_from_C_to_N (si, NULL); break; case 6: // keep going x[si] = hi[si]; state[si] = true; lcp.transfer_i_from_C_to_N (si, NULL); break; } if (cmd <= 3) break; } // for (;;) } // else if (s_error) { break; } } // for (int i=adj_nub; i<n; ++i) lcp.unpermute(); if (!outer_w) delete[] w; delete[] L; delete[] d; delete[] delta_w; delete[] delta_x; delete[] Dell; delete[] ell; #ifdef ROWPTRS delete[] Arows; #endif delete[] p; delete[] C; delete[] state; }
static void CG_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body, dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b, dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, dxQuickStepParameters *qs) { int i,j; const int num_iterations = qs->num_iterations; // precompute iMJ = inv(M)*J' dRealAllocaArray (iMJ,m*12); compute_invM_JT (m,J,iMJ,jb,body,invI); dReal last_rho = 0; dRealAllocaArray (r,m); dRealAllocaArray (z,m); dRealAllocaArray (p,m); dRealAllocaArray (q,m); // precompute 1 / diagonals of A dRealAllocaArray (Ad,m); dRealPtr iMJ_ptr = iMJ; dRealPtr J_ptr = J; for (i=0; i<m; i++) { dReal sum = 0; for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j]; if (jb[i*2+1] >= 0) { for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; } iMJ_ptr += 12; J_ptr += 12; Ad[i] = REAL(1.0) / (sum + cfm[i]); } #ifdef WARM_STARTING // compute residual r = b - A*lambda multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r); for (i=0; i<m; i++) r[i] = b[i] - r[i]; #else dSetZero (lambda,m); memcpy (r,b,m*sizeof(dReal)); // residual r = b - A*lambda #endif for (int iteration=0; iteration < num_iterations; iteration++) { for (i=0; i<m; i++) z[i] = r[i]*Ad[i]; // z = inv(M)*r dReal rho = dot (m,r,z); // rho = r'*z // @@@ // we must check for convergence, otherwise rho will go to 0 if // we get an exact solution, which will introduce NaNs into the equations. if (rho < 1e-10) { printf ("CG returned at iteration %d\n",iteration); break; } if (iteration==0) { memcpy (p,z,m*sizeof(dReal)); // p = z } else { add (m,p,z,p,rho/last_rho); // p = z + (rho/last_rho)*p } // compute q = (J*inv(M)*J')*p multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,p,q); dReal alpha = rho/dot (m,p,q); // alpha = rho/(p'*q) add (m,lambda,lambda,p,alpha); // lambda = lambda + alpha*p add (m,r,r,q,-alpha); // r = r - alpha*q last_rho = rho; } // compute fc = inv(M)*J'*lambda multiply_invM_JT (m,nb,iMJ,jb,lambda,fc); #if 0 // measure solution error multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r); dReal error = 0; for (i=0; i<m; i++) error += dFabs(r[i] - b[i]); printf ("lambda error = %10.6e\n",error); #endif }
void sCylinderBoxData::_cldClipBoxToCylinder() { dIASSERT(m_nContacts != (m_iFlags & NUMC_MASK)); dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; // check which circle from cylinder we take for clipping if ( dVector3Dot(m_vCylinderAxis, m_vNormal) > REAL(0.0) ) { // get top circle vCylinderCirclePos[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); vCylinderCirclePos[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); vCylinderCirclePos[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); vCylinderCircleNormal_Rel[0] = REAL(0.0); vCylinderCircleNormal_Rel[1] = REAL(0.0); vCylinderCircleNormal_Rel[2] = REAL(0.0); vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); } else { // get bottom circle vCylinderCirclePos[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); vCylinderCirclePos[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); vCylinderCirclePos[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); vCylinderCircleNormal_Rel[0] = REAL(0.0); vCylinderCircleNormal_Rel[1] = REAL(0.0); vCylinderCircleNormal_Rel[2] = REAL(0.0); vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); } // vNr is normal in Box frame, pointing from Cylinder to Box dVector3 vNr; dMatrix3 mBoxInv; // Find a way to use quaternion dMatrix3Inv(m_mBoxRot,mBoxInv); dMultiplyMat3Vec3(mBoxInv,m_vNormal,vNr); dVector3 vAbsNormal; vAbsNormal[0] = dFabs( vNr[0] ); vAbsNormal[1] = dFabs( vNr[1] ); vAbsNormal[2] = dFabs( vNr[2] ); // find which face in box is closest to cylinder int iB0, iB1, iB2; // Different from Croteam's code if (vAbsNormal[1] > vAbsNormal[0]) { // 1 > 0 if (vAbsNormal[0]> vAbsNormal[2]) { // 0 > 2 -> 1 > 0 >2 iB0 = 1; iB1 = 0; iB2 = 2; } else { // 2 > 0-> Must compare 1 and 2 if (vAbsNormal[1] > vAbsNormal[2]) { // 1 > 2 -> 1 > 2 > 0 iB0 = 1; iB1 = 2; iB2 = 0; } else { // 2 > 1 -> 2 > 1 > 0; iB0 = 2; iB1 = 1; iB2 = 0; } } } else { // 0 > 1 if (vAbsNormal[1] > vAbsNormal[2]) { // 1 > 2 -> 0 > 1 > 2 iB0 = 0; iB1 = 1; iB2 = 2; } else { // 2 > 1 -> Must compare 0 and 2 if (vAbsNormal[0] > vAbsNormal[2]) { // 0 > 2 -> 0 > 2 > 1; iB0 = 0; iB1 = 2; iB2 = 1; } else { // 2 > 0 -> 2 > 0 > 1; iB0 = 2; iB1 = 0; iB2 = 1; } } } dVector3 vCenter; // find center of box polygon dVector3 vTemp; if (vNr[iB0] > 0) { dMat3GetCol(m_mBoxRot,iB0,vTemp); vCenter[0] = m_vBoxPos[0] - m_vBoxHalfSize[iB0]*vTemp[0]; vCenter[1] = m_vBoxPos[1] - m_vBoxHalfSize[iB0]*vTemp[1]; vCenter[2] = m_vBoxPos[2] - m_vBoxHalfSize[iB0]*vTemp[2]; } else { dMat3GetCol(m_mBoxRot,iB0,vTemp); vCenter[0] = m_vBoxPos[0] + m_vBoxHalfSize[iB0]*vTemp[0]; vCenter[1] = m_vBoxPos[1] + m_vBoxHalfSize[iB0]*vTemp[1]; vCenter[2] = m_vBoxPos[2] + m_vBoxHalfSize[iB0]*vTemp[2]; } // find the vertices of box polygon dVector3 avPoints[4]; dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS]; dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS]; int i=0; for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++) { avTempArray1[i][0] = REAL(0.0); avTempArray1[i][1] = REAL(0.0); avTempArray1[i][2] = REAL(0.0); avTempArray2[i][0] = REAL(0.0); avTempArray2[i][1] = REAL(0.0); avTempArray2[i][2] = REAL(0.0); } dVector3 vAxis1, vAxis2; dMat3GetCol(m_mBoxRot,iB1,vAxis1); dMat3GetCol(m_mBoxRot,iB2,vAxis2); avPoints[0][0] = vCenter[0] + m_vBoxHalfSize[iB1] * vAxis1[0] - m_vBoxHalfSize[iB2] * vAxis2[0]; avPoints[0][1] = vCenter[1] + m_vBoxHalfSize[iB1] * vAxis1[1] - m_vBoxHalfSize[iB2] * vAxis2[1]; avPoints[0][2] = vCenter[2] + m_vBoxHalfSize[iB1] * vAxis1[2] - m_vBoxHalfSize[iB2] * vAxis2[2]; avPoints[1][0] = vCenter[0] - m_vBoxHalfSize[iB1] * vAxis1[0] - m_vBoxHalfSize[iB2] * vAxis2[0]; avPoints[1][1] = vCenter[1] - m_vBoxHalfSize[iB1] * vAxis1[1] - m_vBoxHalfSize[iB2] * vAxis2[1]; avPoints[1][2] = vCenter[2] - m_vBoxHalfSize[iB1] * vAxis1[2] - m_vBoxHalfSize[iB2] * vAxis2[2]; avPoints[2][0] = vCenter[0] - m_vBoxHalfSize[iB1] * vAxis1[0] + m_vBoxHalfSize[iB2] * vAxis2[0]; avPoints[2][1] = vCenter[1] - m_vBoxHalfSize[iB1] * vAxis1[1] + m_vBoxHalfSize[iB2] * vAxis2[1]; avPoints[2][2] = vCenter[2] - m_vBoxHalfSize[iB1] * vAxis1[2] + m_vBoxHalfSize[iB2] * vAxis2[2]; avPoints[3][0] = vCenter[0] + m_vBoxHalfSize[iB1] * vAxis1[0] + m_vBoxHalfSize[iB2] * vAxis2[0]; avPoints[3][1] = vCenter[1] + m_vBoxHalfSize[iB1] * vAxis1[1] + m_vBoxHalfSize[iB2] * vAxis2[1]; avPoints[3][2] = vCenter[2] + m_vBoxHalfSize[iB1] * vAxis1[2] + m_vBoxHalfSize[iB2] * vAxis2[2]; // transform box points to space of cylinder circle dMatrix3 mCylinderInv; dMatrix3Inv(m_mCylinderRot,mCylinderInv); for(i=0; i<4; i++) { dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp); dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]); } int iTmpCounter1 = 0; int iTmpCounter2 = 0; dVector4 plPlane; // plane of cylinder that contains circle for intersection dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane); // Body of base circle of Cylinder int nCircleSegment = 0; for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; nCircleSegment++) { dConstructPlane(m_avCylinderNormals[nCircleSegment],m_fCylinderRadius,plPlane); if (0 == (nCircleSegment % 2)) { dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane); } else { dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane ); } dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= MAX_CYLBOX_CLIP_POINTS ); dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS ); } // back transform clipped points to absolute space dReal ftmpdot; dReal fTempDepth; dVector3 vPoint; if (nCircleSegment % 2) { for( i=0; i<iTmpCounter2; i++) { dMULTIPLY0_331(vPoint,m_mCylinderRot,avTempArray2[i]); vPoint[0] += vCylinderCirclePos[0]; vPoint[1] += vCylinderCirclePos[1]; vPoint[2] += vCylinderCirclePos[2]; dVector3Subtract(vPoint,m_vCylinderPos,vTemp); ftmpdot = dVector3Dot(vTemp, m_vNormal); fTempDepth = m_fBestrc - ftmpdot; // Depth must be positive if (fTempDepth > REAL(0.0)) { // generate contacts dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip); Contact0->depth = fTempDepth; dVector3Copy(m_vNormal,Contact0->normal); dVector3Copy(vPoint,Contact0->pos); Contact0->g1 = m_gCylinder; Contact0->g2 = m_gBox; Contact0->side1 = -1; Contact0->side2 = -1; dVector3Inv(Contact0->normal); m_nContacts++; if (m_nContacts == (m_iFlags & NUMC_MASK)) { break; } } } } else { for( i=0; i<iTmpCounter1; i++) { dMULTIPLY0_331(vPoint,m_mCylinderRot,avTempArray1[i]); vPoint[0] += vCylinderCirclePos[0]; vPoint[1] += vCylinderCirclePos[1]; vPoint[2] += vCylinderCirclePos[2]; dVector3Subtract(vPoint,m_vCylinderPos,vTemp); ftmpdot = dVector3Dot(vTemp, m_vNormal); fTempDepth = m_fBestrc - ftmpdot; // Depth must be positive if (fTempDepth > REAL(0.0)) { // generate contacts dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip); Contact0->depth = fTempDepth; dVector3Copy(m_vNormal,Contact0->normal); dVector3Copy(vPoint,Contact0->pos); Contact0->g1 = m_gCylinder; Contact0->g2 = m_gBox; Contact0->side1 = -1; Contact0->side2 = -1; dVector3Inv(Contact0->normal); m_nContacts++; if (m_nContacts == (m_iFlags & NUMC_MASK)) { break; } } } } }
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_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_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 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(); }
BOOL sTrimeshCapsuleColliderData::_cldTestAxis( const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, dVector3 vAxis, int iAxis, BOOL bNoFlip/* = FALSE*/) { // calculate length of separating axis vector dReal fL = LENGTHOF(vAxis); // if not long enough // TODO : dReal epsilon please if ( fL < REAL(1e-5) ) { // do nothing //iLastOutAxis = 0; return TRUE; } // otherwise normalize it dNormalize3(vAxis); // project capsule on vAxis dReal frc = dFabs(dCalcVectorDot3(m_vCapsuleAxis,vAxis))*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius) + m_vCapsuleRadius; // project triangle on vAxis dReal afv[3]; afv[0] = dCalcVectorDot3(m_vV0, vAxis); afv[1] = dCalcVectorDot3(m_vV1, vAxis); afv[2] = dCalcVectorDot3(m_vV2, vAxis); dReal fMin = MAX_REAL; dReal fMax = MIN_REAL; // for each vertex for(int i=0; i<3; i++) { // find minimum if (afv[i]<fMin) { fMin = afv[i]; } // find maximum if (afv[i]>fMax) { fMax = afv[i]; } } // find triangle's center of interval on axis dReal fCenter = (fMin+fMax)*REAL(0.5); // calculate triangles half interval dReal fTriangleRadius = (fMax-fMin)*REAL(0.5); // if they do not overlap, if (dFabs(fCenter) > ( frc + fTriangleRadius )) { // exit, we have no intersection return FALSE; } // calculate depth dReal fDepth = dFabs(fCenter) - (frc+fTriangleRadius); // if greater then best found so far if ( fDepth > m_fBestDepth ) { // remember depth m_fBestDepth = fDepth; m_fBestCenter = fCenter; m_fBestrt = fTriangleRadius; m_vNormal[0] = vAxis[0]; m_vNormal[1] = vAxis[1]; m_vNormal[2] = vAxis[2]; m_iBestAxis = iAxis; // flip normal if interval is wrong faced if (fCenter<0 && !bNoFlip) { m_vNormal[0] = -m_vNormal[0]; m_vNormal[1] = -m_vNormal[1]; m_vNormal[2] = -m_vNormal[2]; m_fBestCenter = -fCenter; } } return TRUE; }
int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (o1->type == dSphereClass); dIASSERT (o2->type == dBoxClass); dIASSERT ((flags & NUMC_MASK) >= 1); // this is easy. get the sphere center `p' relative to the box, and then clip // that to the boundary of the box (call that point `q'). if q is on the // boundary of the box and |p-q| is <= sphere radius, they touch. // if q is inside the box, the sphere is inside the box, so set a contact // normal to push the sphere to the closest box face. dVector3 l,t,p,q,r; dReal depth; int onborder = 0; dxSphere *sphere = (dxSphere*) o1; dxBox *box = (dxBox*) o2; contact->g1 = o1; contact->g2 = o2; p[0] = o1->final_posr->pos[0] - o2->final_posr->pos[0]; p[1] = o1->final_posr->pos[1] - o2->final_posr->pos[1]; p[2] = o1->final_posr->pos[2] - o2->final_posr->pos[2]; l[0] = box->side[0]*REAL(0.5); t[0] = dDOT14(p,o2->final_posr->R); if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; } if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; } l[1] = box->side[1]*REAL(0.5); t[1] = dDOT14(p,o2->final_posr->R+1); if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; } if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; } t[2] = dDOT14(p,o2->final_posr->R+2); l[2] = box->side[2]*REAL(0.5); if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; } if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; } if (!onborder) { // sphere center inside box. find closest face to `t' dReal min_distance = l[0] - dFabs(t[0]); int mini = 0; for (int i=1; i<3; i++) { dReal face_distance = l[i] - dFabs(t[i]); if (face_distance < min_distance) { min_distance = face_distance; mini = i; } } // contact position = sphere center contact->pos[0] = o1->final_posr->pos[0]; contact->pos[1] = o1->final_posr->pos[1]; contact->pos[2] = o1->final_posr->pos[2]; // contact normal points to closest face dVector3 tmp; tmp[0] = 0; tmp[1] = 0; tmp[2] = 0; tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0); dMULTIPLY0_331 (contact->normal,o2->final_posr->R,tmp); // contact depth = distance to wall along normal plus radius contact->depth = min_distance + sphere->radius; return 1; } t[3] = 0; //@@@ hmmm dMULTIPLY0_331 (q,o2->final_posr->R,t); r[0] = p[0] - q[0]; r[1] = p[1] - q[1]; r[2] = p[2] - q[2]; depth = sphere->radius - dSqrt(dDOT(r,r)); if (depth < 0) return 0; contact->pos[0] = q[0] + o2->final_posr->pos[0]; contact->pos[1] = q[1] + o2->final_posr->pos[1]; contact->pos[2] = q[2] + o2->final_posr->pos[2]; contact->normal[0] = r[0]; contact->normal[1] = r[1]; contact->normal[2] = r[2]; dNormalize3 (contact->normal); contact->depth = depth; return 1; }
// test for given separating axis int sCylinderBoxData::_cldTestAxis( dVector3& vInputNormal, int iAxis ) { // check length of input normal dReal fL = dVector3Length(vInputNormal); // if not long enough if ( fL < REAL(1e-5) ) { // do nothing return 1; } // otherwise make it unit for sure dNormalize3(vInputNormal); // project box and Cylinder on mAxis dReal fdot1 = dVector3Dot(m_vCylinderAxis, vInputNormal); dReal frc; if (fdot1 > REAL(1.0)) { // assume fdot1 = 1 frc = m_fCylinderSize*REAL(0.5); } else if (fdot1 < REAL(-1.0)) { // assume fdot1 = -1 frc = m_fCylinderSize*REAL(0.5); } else { // project box and capsule on iAxis frc = dFabs( fdot1 * (m_fCylinderSize*REAL(0.5))) + m_fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); } dVector3 vTemp1; dMat3GetCol(m_mBoxRot,0,vTemp1); dReal frb = dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[0]; dMat3GetCol(m_mBoxRot,1,vTemp1); frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[1]; dMat3GetCol(m_mBoxRot,2,vTemp1); frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[2]; // project their distance on separating axis dReal fd = dVector3Dot(m_vDiff,vInputNormal); // get depth dReal fDepth = frc + frb; // Calculate partial depth // if they do not overlap exit, we have no intersection if ( dFabs(fd) > fDepth ) { return 0; } // Finalyze the depth calculation fDepth -= dFabs(fd); // get maximum depth if ( fDepth < m_fBestDepth ) { m_fBestDepth = fDepth; dVector3Copy(vInputNormal,m_vNormal); m_iBestAxis = iAxis; m_fBestrb = frb; m_fBestrc = frc; // flip normal if interval is wrong faced if (fd > 0) { dVector3Inv(m_vNormal); } } return 1; }
// Ray-Cylinder collider by Joseph Cooper (2011) int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip ) { dIASSERT( skip >= (int)sizeof( dContactGeom ) ); dIASSERT( o1->type == dRayClass ); dIASSERT( o2->type == dCylinderClass ); dIASSERT( (flags & NUMC_MASK) >= 1 ); dxRay* ray = (dxRay*)( o1 ); dxCylinder* cyl = (dxCylinder*)( o2 ); // Fill in contact information. contact->g1 = ray; contact->g2 = cyl; contact->side1 = -1; contact->side2 = -1; const dReal half_length = cyl->lz * REAL( 0.5 ); /* Possible collision cases: * Ray origin between/outside caps * Ray origin within/outside radius * Ray direction left/right/perpendicular * Ray direction parallel/perpendicular/other * * Ray origin cases (ignoring origin on surface) * * A B * /-\-----------\ * C ( ) D ) * \_/___________/ * * Cases A and D can collide with caps or cylinder * Case C can only collide with the caps * Case B can only collide with the cylinder * Case D will produce inverted normals * If the ray is perpendicular, only check the cylinder * If the ray is parallel to cylinder axis, * we can only check caps * If the ray points right, * Case A,C Check left cap * Case D Check right cap * If the ray points left * Case A,C Check right cap * Case D Check left cap * Case B, check only first possible cylinder collision * Case D, check only second possible cylinder collision */ // Find the ray in the cylinder coordinate frame: dVector3 tmp; dVector3 pos; // Ray origin in cylinder frame dVector3 dir; // Ray direction in cylinder frame // Translate ray start by inverse cyl dSubtractVectors3(tmp,ray->final_posr->pos,cyl->final_posr->pos); // Rotate ray start by inverse cyl dMultiply1_331(pos,cyl->final_posr->R,tmp); // Get the ray's direction tmp[0] = ray->final_posr->R[2]; tmp[1] = ray->final_posr->R[6]; tmp[2] = ray->final_posr->R[10]; // Rotate the ray direction by inverse cyl dMultiply1_331(dir,cyl->final_posr->R,tmp); // Is the ray origin inside of the (extended) cylinder? dReal r2 = cyl->radius*cyl->radius; dReal C = pos[0]*pos[0] + pos[1]*pos[1] - r2; // Find the different cases // Is ray parallel to the cylinder length? int parallel = (dir[0]==0 && dir[1]==0); // Is ray perpendicular to the cylinder length? int perpendicular = (dir[2]==0); // Is ray origin within the radius of the caps? int inRadius = (C<=0); // Is ray origin between the top and bottom caps? int inCaps = (dFabs(pos[2])<=half_length); int checkCaps = (!perpendicular && (!inCaps || inRadius)); int checkCyl = (!parallel && (!inRadius || inCaps)); int flipNormals = (inCaps&&inRadius); dReal tt=-dInfinity; // Depth to intersection dVector3 tmpNorm = {dNaN, dNaN, dNaN}; // ensure we don't leak garbage if (checkCaps) { // Make it so we only need to check one cap int flipDir = 0; // Wish c had logical xor... if ((dir[2]<0 && flipNormals) || (dir[2]>0 && !flipNormals)) { flipDir = 1; dir[2]=-dir[2]; pos[2]=-pos[2]; } // The cap is half the cylinder's length // from the cylinder's origin // We only checkCaps if dir[2]!=0 tt = (half_length-pos[2])/dir[2]; if (tt>=0 && tt<=ray->length) { tmp[0] = pos[0] + tt*dir[0]; tmp[1] = pos[1] + tt*dir[1]; // Ensure collision point is within cap circle if (tmp[0]*tmp[0] + tmp[1]*tmp[1] <= r2) { // Successful collision tmp[2] = (flipDir)?-half_length:half_length; tmpNorm[0]=0; tmpNorm[1]=0; tmpNorm[2]=(flipDir!=flipNormals)?-1:1; checkCyl = 0; // Short circuit cylinder check } else { // Ray hits cap plane outside of cap circle tt=-dInfinity; // No collision yet } } else { // The cap plane is beyond (or behind) the ray length tt=-dInfinity; // No collision yet } if (flipDir) { // Flip back dir[2]=-dir[2]; pos[2]=-pos[2]; } } if (checkCyl) { // Compute quadratic formula for parametric ray equation dReal A = dir[0]*dir[0] + dir[1]*dir[1]; dReal B = 2*(pos[0]*dir[0] + pos[1]*dir[1]); // Already computed C dReal k = B*B - 4*A*C; // Check collision with infinite cylinder // k<0 means the ray passes outside the cylinder // k==0 means ray is tangent to cylinder (or parallel) // // Our quadratic formula: tt = (-B +- sqrt(k))/(2*A) // // A must be positive (otherwise we wouldn't be checking // cylinder because ray is parallel) // if (k<0) ray doesn't collide with sphere // if (B > sqrt(k)) then both times are negative // -- don't calculate // if (B<-sqrt(k)) then both times are positive (Case A or B) // -- only calculate first, if first isn't valid // -- second can't be without first going through a cap // otherwise (fabs(B)<=sqrt(k)) then C<=0 (ray-origin inside/on cylinder) // -- only calculate second collision if (k>=0 && (B<0 || B*B<=k)) { k = dSqrt(k); A = dRecip(2*A); if (dFabs(B)<=k) { tt = (-B + k)*A; // Second solution // If ray origin is on surface and pointed out, we // can get a tt=0 solution... } else { tt = (-B - k)*A; // First solution } if (tt<=ray->length) { tmp[2] = pos[2] + tt*dir[2]; if (dFabs(tmp[2])<=half_length) { // Valid solution tmp[0] = pos[0] + tt*dir[0]; tmp[1] = pos[1] + tt*dir[1]; tmpNorm[0] = tmp[0]/cyl->radius; tmpNorm[1] = tmp[1]/cyl->radius; tmpNorm[2] = 0; if (flipNormals) { // Ray origin was inside cylinder tmpNorm[0] = -tmpNorm[0]; tmpNorm[1] = -tmpNorm[1]; } } else { // Ray hits cylinder outside of caps tt=-dInfinity; } } else { // Ray doesn't reach the cylinder tt=-dInfinity; } } } if (tt>0) { contact->depth = tt; // Transform the point back to world coordinates tmpNorm[3]=0; tmp[3] = 0; dMultiply0_331(contact->normal,cyl->final_posr->R,tmpNorm); dMultiply0_331(contact->pos,cyl->final_posr->R,tmp); contact->pos[0]+=cyl->final_posr->pos[0]; contact->pos[1]+=cyl->final_posr->pos[1]; contact->pos[2]+=cyl->final_posr->pos[2]; return 1; } // No contact with anything. return 0; }
void cullPoints (int n, dReal p[], int m, int i0, int iret[]) { // compute the centroid of the polygon in cx,cy int i,j; dReal a,cx,cy,q; if (n==1) { cx = p[0]; cy = p[1]; } else if (n==2) { cx = REAL(0.5)*(p[0] + p[2]); cy = REAL(0.5)*(p[1] + p[3]); } else { a = 0; cx = 0; cy = 0; for (i=0; i<(n-1); i++) { q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; a += q; cx += q*(p[i*2]+p[i*2+2]); cy += q*(p[i*2+1]+p[i*2+3]); } q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; a = dRecip(REAL(3.0)*(a+q)); cx = a*(cx + q*(p[n*2-2]+p[0])); cy = a*(cy + q*(p[n*2-1]+p[1])); } // compute the angle of each point w.r.t. the centroid dReal A[8]; for (i=0; i<n; i++) A[i] = dAtan2(p[i*2+1]-cy,p[i*2]-cx); // search for points that have angles closest to A[i0] + i*(2*pi/m). int avail[8]; for (i=0; i<n; i++) avail[i] = 1; avail[i0] = 0; iret[0] = i0; iret++; for (j=1; j<m; j++) { a = (dReal)(dReal(j)*(2*M_PI/m) + A[i0]); if (a > M_PI) a -= (dReal)(2*M_PI); dReal maxdiff=1e9,diff; #ifndef dNODEBUG *iret = i0; // iret is not allowed to keep this value #endif for (i=0; i<n; i++) { if (avail[i]) { diff = dFabs (A[i]-a); if (diff > M_PI) diff = (dReal) (2*M_PI - diff); if (diff < maxdiff) { maxdiff = diff; *iret = i; } } } #ifndef dNODEBUG dIASSERT (*iret != i0); // ensure iret got set #endif avail[*iret] = 0; iret++; } }
int dcTriListCollider::dTriCyl ( const dReal* v0,const dReal* v1,const dReal* v2, Triangle* T, dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip ) { // VERIFY (skip >= (int)sizeof(dContactGeom)); VERIFY (dGeomGetClass(o1)== dCylinderClassUser); const dReal *R = dGeomGetRotation(o1); const dReal* p=dGeomGetPosition(o1); dReal radius; dReal hlz; dGeomCylinderGetParams(o1,&radius,&hlz); hlz/=2.f; // find number of contacts requested int maxc = flags & NUMC_MASK; if (maxc < 1) maxc = 1; if (maxc > 3) maxc = 3; // no more than 3 contacts per box allowed const dVector3 &triAx=T->norm; dVector3 triSideAx0={T->side0[0],T->side0[1],T->side0[2]}; //{v1[0]-v0[0],v1[1]-v0[1],v1[2]-v0[2]}; dVector3 triSideAx1={T->side1[0],T->side1[1],T->side1[2]}; //{v2[0]-v1[0],v2[1]-v1[1],v2[2]-v1[2]}; dVector3 triSideAx2={v0[0]-v2[0],v0[1]-v2[1],v0[2]-v2[2]}; //dCROSS(triAx,=,triSideAx0,triSideAx1); int code=0; dReal signum, outDepth,cos0,cos1,cos2,sin1; //////////////////////////////////////////////////////////////////////////// //sepparation along tri plane normal;/////////////////////////////////////// //////////////////////////////////////////////////////////////////////////// //accurate_normalize(triAx); //cos0=dDOT14(triAx,R+0); cos1=dFabs(dDOT14(triAx,R+1)); //cos2=dDOT14(triAx,R+2); //sin1=_sqrt(cos0*cos0+cos2*cos2); //////////////////////// //another way ////////// cos1=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f sin1=_sqrt(REAL(1.)-cos1*cos1); ////////////////////////////// dReal sidePr=cos1*hlz+sin1*radius; dReal dist=-T->dist; //dDOT(triAx,v0)-dDOT(triAx,p); if(dist>0.f) RETURN0; dReal depth=sidePr-dFabs(dist); outDepth=depth; signum=dist>0.f ? 1.f : -1.f; code=0; if(depth<0.f) RETURN0; dReal depth0,depth1,depth2,dist0,dist1,dist2; bool isPdist0,isPdist1,isPdist2; bool testV0,testV1,testV2; bool sideTestV00,sideTestV01,sideTestV02; bool sideTestV10,sideTestV11,sideTestV12; bool sideTestV20,sideTestV21,sideTestV22; ////////////////////////////////////////////////////////////////////////////// //cylinder axis - one of the triangle vertexes touches cylinder's flat surface ////////////////////////////////////////////////////////////////////////////// dist0=dDOT14(v0,R+1)-dDOT14(p,R+1); dist1=dDOT14(v1,R+1)-dDOT14(p,R+1); dist2=dDOT14(v2,R+1)-dDOT14(p,R+1); isPdist0=dist0>0.f; isPdist1=dist1>0.f; isPdist2=dist2>0.f; depth0=hlz-dFabs(dist0); depth1=hlz-dFabs(dist1); depth2=hlz-dFabs(dist2); testV0=depth0>0.f; testV1=depth1>0.f; testV2=depth2>0.f; if(isPdist0==isPdist1 && isPdist1== isPdist2) //(here and lower) check the tryangle is on one side of the cylinder { if(depth0>depth1) if(depth0>depth2) if(testV0){ if(depth0<outDepth) { signum= isPdist0 ? 1.f : -1.f; outDepth=depth0; code=1; } } else RETURN0; else if(testV2){ if(depth2<outDepth) { outDepth=depth2; signum= isPdist2 ? 1.f : -1.f; code=3; } } else RETURN0; else if(depth1>depth2) if(testV1){ if(depth1<outDepth) { outDepth=depth1; signum= isPdist1 ? 1.f : -1.f; code=2; } } else RETURN0; else if(testV2){ if(depth2<outDepth) { outDepth=depth2; signum= isPdist2 ? 1.f : -1.f; code=2; } } else RETURN0; } dVector3 axis,outAx; dReal posProj; dReal pointDepth=0.f; #define TEST(vx,ox1,ox2,c) \ {\ posProj=dDOT14(v##vx,R+1)-dDOT14(p,R+1);\ \ axis[0]=v##vx[0]-p[0]-R[1]*posProj;\ axis[1]=v##vx[1]-p[1]-R[5]*posProj;\ axis[2]=v##vx[2]-p[2]-R[9]*posProj;\ \ accurate_normalize(axis);\ \ \ dist0=dDOT(v0,axis)-dDOT(p,axis);\ dist1=dDOT(v1,axis)-dDOT(p,axis);\ dist2=dDOT(v2,axis)-dDOT(p,axis);\ \ isPdist0=dist0>0.f;\ isPdist1=dist1>0.f;\ isPdist2=dist2>0.f;\ \ depth0=radius-dFabs(dist0);\ depth1=radius-dFabs(dist1);\ depth2=radius-dFabs(dist2);\ \ sideTestV##vx##0=depth0>0.f;\ sideTestV##vx##1=depth1>0.f;\ sideTestV##vx##2=depth2>0.f;\ \ if(isPdist0==isPdist1 && isPdist1== isPdist2)\ \ {\ if(sideTestV##vx##0||sideTestV##vx##1||sideTestV##vx##2){\ if(!(depth##vx<depth##ox1 || depth##vx<depth##ox2))\ {\ if(depth##vx<outDepth && depth##vx > pointDepth)\ {\ pointDepth=depth##vx;\ signum= isPdist##vx ? 1.f : -1.f;\ outAx[0]=axis[0];\ outAx[1]=axis[1];\ outAx[2]=axis[2];\ code=c;\ }\ }\ }\ else RETURN0;\ \ \ \ }\ } if(testV0) TEST(0,1,2,4) if(testV1 ) TEST(1,2,0,5) //&& sideTestV01 if(testV2 ) TEST(2,0,1,6) //&& sideTestV02 && sideTestV12 #undef TEST dVector3 tpos,pos; if(code>3) outDepth=pointDepth; //deepest vertex axis used if its depth less than outDepth //else{ //bool outV0=!(testV0&&sideTestV00&&sideTestV10&&sideTestV20); //bool outV1=!(testV1&&sideTestV01&&sideTestV11&&sideTestV21); //bool outV2=!(testV2&&sideTestV02&&sideTestV12&&sideTestV22); bool outV0=true; bool outV1=true; bool outV2=true; ///////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// ///crosses between triangle sides and cylinder axis////////////////////////// ///////////////////////////////////////////////////////////////////////////// #define TEST(ax,nx,ox,c) if(cylinderCrossesLine(p,R+1,hlz,v##ax,v##nx,triSideAx##ax,tpos)) {\ dCROSS114(axis,=,triSideAx##ax,R+1);\ accurate_normalize(axis);\ dist##ax=dDOT(v##ax,axis)-dDOT(p,axis);\ dist##ox=dDOT(v##ox,axis)-dDOT(p,axis);\ \ isPdist##ax=dist##ax>0.f;\ isPdist##ox=dist##ox>0.f;\ \ if(isPdist##ax == isPdist##ox)\ {\ depth##ax=radius-dFabs(dist##ax);\ depth##ox=radius-dFabs(dist##ox);\ \ if(depth##ax>0.f){\ if(depth##ax<=outDepth && depth##ax>=depth##ox) \ {\ outDepth=depth##ax;\ signum= isPdist##ax ? 1.f : -1.f;\ outAx[0]=axis[0];\ outAx[1]=axis[1];\ outAx[2]=axis[2];\ pos[0]=tpos[0];\ pos[1]=tpos[1];\ pos[2]=tpos[2];\ code=c;\ }\ }\ else if(depth##ox<0.f) RETURN0;\ \ }\ } accurate_normalize(triSideAx0); if(outV0&&outV1) TEST(0,1,2,7) accurate_normalize(triSideAx1); if(outV1&&outV2) TEST(1,2,0,8) accurate_normalize(triSideAx2); if(outV2&&outV0) TEST(2,0,1,9) #undef TEST //////////////////////////////////// //test cylinder rings on triangle sides//// //////////////////////////////////// dVector3 tAx,cen; dReal sign; bool cs; #define TEST(ax,nx,ox,c) \ {\ posProj=dDOT(p,triSideAx##ax)-dDOT(v##ax,triSideAx##ax);\ axis[0]=p[0]-v0[0]-triSideAx##ax[0]*posProj;\ axis[1]=p[1]-v0[1]-triSideAx##ax[1]*posProj;\ axis[2]=p[2]-v0[2]-triSideAx##ax[2]*posProj;\ \ sign=dDOT14(axis,R+1)>0.f ? 1.f :-1.f;\ cen[0]=p[0]-sign*R[1]*hlz;\ cen[1]=p[1]-sign*R[5]*hlz;\ cen[2]=p[2]-sign*R[9]*hlz;\ \ cs=circleLineIntersection(R+1,cen,radius,triSideAx##ax,v##ax,-sign,tpos);\ \ axis[0]=tpos[0]-cen[0];\ axis[1]=tpos[1]-cen[1];\ axis[2]=tpos[2]-cen[2];\ \ if(cs){ \ \ cos0=dDOT14(axis,R+0);\ cos2=dDOT14(axis,R+2);\ tAx[0]=R[2]*cos0-R[0]*cos2;\ tAx[1]=R[6]*cos0-R[4]*cos2;\ tAx[2]=R[10]*cos0-R[8]*cos2;\ \ dCROSS(axis,=,triSideAx##ax,tAx);\ \ }\ accurate_normalize(axis);\ dist##ax=dDOT(v##ax,axis)-dDOT(p,axis);\ if(dist##ax*dDOT(axis,triSideAx##nx)>0.f){\ \ cos0=dDOT14(axis,R+0);\ cos1=dFabs(dDOT14(axis,R+1));\ cos2=dDOT14(axis,R+2);\ \ \ sin1=_sqrt(cos0*cos0+cos2*cos2);\ \ sidePr=cos1*hlz+sin1*radius;\ \ \ dist##ox=dDOT(v##ox,axis)-dDOT(p,axis);\ \ isPdist##ax=dist##ax>0.f;\ isPdist##ox=dist##ox>0.f;\ \ if(isPdist##ax == isPdist##ox) \ \ {\ depth##ax=sidePr-dFabs(dist##ax);\ depth##ox=sidePr-dFabs(dist##ox);\ \ if(depth##ax>0.f){\ if(depth##ax<outDepth) \ {\ outDepth=depth##ax;\ signum= isPdist##ax ? 1.f : -1.f;\ outAx[0]=axis[0];\ outAx[1]=axis[1];\ outAx[2]=axis[2];\ pos[0]=tpos[0];\ pos[1]=tpos[1];\ pos[2]=tpos[2];\ code=c;\ }\ }\ else if(depth##ox<0.f) RETURN0;\ \ \ }\ }\ } if(7!=code) TEST(0,1,2,10) if(8!=code) TEST(1,2,0,11) if(9!=code) TEST(2,0,1,12) #undef TEST //} ////////////////////////////////////////////////////////////////////// ///if we get to this poit tri touches cylinder/////////////////////// ///////////////////////////////////////////////////////////////////// //VERIFY( g_pGameLevel ); CDB::TRI* T_array = inl_ph_world().ObjectSpace().GetStaticTris(); dVector3 norm; unsigned int ret; flags8& gl_state=gl_cl_tries_state[I-B]; if(code==0){ norm[0]=triAx[0]*signum; norm[1]=triAx[1]*signum; norm[2]=triAx[2]*signum; dReal Q1 = dDOT14(norm,R+0); dReal Q2 = dDOT14(norm,R+1); dReal Q3 = dDOT14(norm,R+2); dReal factor =_sqrt(Q1*Q1+Q3*Q3); dReal C1,C3; dReal centerDepth;//depth in the cirle centre if(factor>0.f) { C1=Q1/factor; C3=Q3/factor; } else { C1=1.f; C3=0.f; } dReal A1 = radius * C1;//cosinus dReal A2 = hlz;//Q2 dReal A3 = radius * C3;//sinus if(factor>0.f) centerDepth=outDepth-A1*Q1-A3*Q3; else centerDepth=outDepth; pos[0]=p[0]; pos[1]=p[1]; pos[2]=p[2]; pos[0]+= Q2>0 ? hlz*R[1]:-hlz*R[1]; pos[1]+= Q2>0 ? hlz*R[5]:-hlz*R[5]; pos[2]+= Q2>0 ? hlz*R[9]:-hlz*R[9]; ret=0; dVector3 cross0, cross1, cross2; dReal ds0,ds1,ds2; dCROSS(cross0,=,triAx,triSideAx0); ds0=dDOT(cross0,v0); dCROSS(cross1,=,triAx,triSideAx1); ds1=dDOT(cross1,v1); dCROSS(cross2,=,triAx,triSideAx2); ds2=dDOT(cross2,v2); contact->pos[0] = pos[0]+A1*R[0]+A3*R[2]; contact->pos[1] = pos[1]+A1*R[4]+A3*R[6]; contact->pos[2] = pos[2]+A1*R[8]+A3*R[10]; if(dDOT(cross0,contact->pos)-ds0>0.f && dDOT(cross1,contact->pos)-ds1>0.f && dDOT(cross2,contact->pos)-ds2>0.f){ contact->depth = outDepth; ret=1; } if(dFabs(Q2)>M_SQRT1_2){ A1=(-C1*M_COS_PI_3-C3*M_SIN_PI_3)*radius; A3=(-C3*M_COS_PI_3+C1*M_SIN_PI_3)*radius; CONTACT(contact,ret*skip)->pos[0]=pos[0]+A1*R[0]+A3*R[2]; CONTACT(contact,ret*skip)->pos[1]=pos[1]+A1*R[4]+A3*R[6]; CONTACT(contact,ret*skip)->pos[2]=pos[2]+A1*R[8]+A3*R[10]; CONTACT(contact,ret*skip)->depth=centerDepth+Q1*A1+Q3*A3; if(CONTACT(contact,ret*skip)->depth>0.f) if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f && dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f && dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret; A1=(-C1*M_COS_PI_3+C3*M_SIN_PI_3)*radius; A3=(-C3*M_COS_PI_3-C1*M_SIN_PI_3)*radius; CONTACT(contact,ret*skip)->pos[0]=pos[0]+A1*R[0]+A3*R[2]; CONTACT(contact,ret*skip)->pos[1]=pos[1]+A1*R[4]+A3*R[6]; CONTACT(contact,ret*skip)->pos[2]=pos[2]+A1*R[8]+A3*R[10]; CONTACT(contact,ret*skip)->depth=centerDepth+Q1*A1+Q3*A3; if(CONTACT(contact,ret*skip)->depth>0.f) if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f && dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f && dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret; } else { CONTACT(contact,ret*skip)->pos[0]=contact->pos[0]-2.f*(Q2>0 ? hlz*R[1]:-hlz*R[1]); CONTACT(contact,ret*skip)->pos[1]=contact->pos[1]-2.f*(Q2>0 ? hlz*R[5]:-hlz*R[5]); CONTACT(contact,ret*skip)->pos[2]=contact->pos[2]-2.f*(Q2>0 ? hlz*R[9]:-hlz*R[9]); CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2); if(CONTACT(contact,ret*skip)->depth>0.f) if(dDOT(cross0,CONTACT(contact,ret*skip)->pos)-ds0>0.f && dDOT(cross1,CONTACT(contact,ret*skip)->pos)-ds1>0.f && dDOT(cross2,CONTACT(contact,ret*skip)->pos)-ds2>0.f) ++ret; } }
int dCollideBoxPlane (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (o1->type == dBoxClass); dIASSERT (o2->type == dPlaneClass); dIASSERT ((flags & NUMC_MASK) >= 1); dxBox *box = (dxBox*) o1; dxPlane *plane = (dxPlane*) o2; contact->g1 = o1; contact->g2 = o2; contact->side1 = -1; contact->side2 = -1; int ret = 0; //@@@ problem: using 4-vector (plane->p) as 3-vector (normal). const dReal *R = o1->final_posr->R; // rotation of box const dReal *n = plane->p; // normal vector // project sides lengths along normal vector, get absolute values dReal Q1 = dDOT14(n,R+0); dReal Q2 = dDOT14(n,R+1); dReal Q3 = dDOT14(n,R+2); dReal A1 = box->side[0] * Q1; dReal A2 = box->side[1] * Q2; dReal A3 = box->side[2] * Q3; dReal B1 = dFabs(A1); dReal B2 = dFabs(A2); dReal B3 = dFabs(A3); // early exit test dReal depth = plane->p[3] + REAL(0.5)*(B1+B2+B3) - dDOT(n,o1->final_posr->pos); if (depth < 0) return 0; // find number of contacts requested int maxc = flags & NUMC_MASK; // if (maxc < 1) maxc = 1; // an assertion is made on entry if (maxc > 3) maxc = 3; // not more than 3 contacts per box allowed // find deepest point dVector3 p; p[0] = o1->final_posr->pos[0]; p[1] = o1->final_posr->pos[1]; p[2] = o1->final_posr->pos[2]; #define FOO(i,op) \ p[0] op REAL(0.5)*box->side[i] * R[0+i]; \ p[1] op REAL(0.5)*box->side[i] * R[4+i]; \ p[2] op REAL(0.5)*box->side[i] * R[8+i]; #define BAR(i,iinc) if (A ## iinc > 0) { FOO(i,-=) } else { FOO(i,+=) } BAR(0,1); BAR(1,2); BAR(2,3); #undef FOO #undef BAR // the deepest point is the first contact point contact->pos[0] = p[0]; contact->pos[1] = p[1]; contact->pos[2] = p[2]; contact->normal[0] = n[0]; contact->normal[1] = n[1]; contact->normal[2] = n[2]; contact->depth = depth; ret = 1; // ret is number of contact points found so far if (maxc == 1) goto done; // get the second and third contact points by starting from `p' and going // along the two sides with the smallest projected length. #define FOO(i,j,op) \ CONTACT(contact,i*skip)->pos[0] = p[0] op box->side[j] * R[0+j]; \ CONTACT(contact,i*skip)->pos[1] = p[1] op box->side[j] * R[4+j]; \ CONTACT(contact,i*skip)->pos[2] = p[2] op box->side[j] * R[8+j]; #define BAR(ctact,side,sideinc) \ depth -= B ## sideinc; \ if (depth < 0) goto done; \ if (A ## sideinc > 0) { FOO(ctact,side,+); } else { FOO(ctact,side,-); } \ CONTACT(contact,ctact*skip)->depth = depth; \ ret++; CONTACT(contact,skip)->normal[0] = n[0]; CONTACT(contact,skip)->normal[1] = n[1]; CONTACT(contact,skip)->normal[2] = n[2]; if (maxc == 3) { CONTACT(contact,2*skip)->normal[0] = n[0]; CONTACT(contact,2*skip)->normal[1] = n[1]; CONTACT(contact,2*skip)->normal[2] = n[2]; } if (B1 < B2) { if (B3 < B1) goto use_side_3; else { BAR(1,0,1); // use side 1 if (maxc == 2) goto done; if (B2 < B3) goto contact2_2; else goto contact2_3; } } else { if (B3 < B2) { use_side_3: // use side 3 BAR(1,2,3); if (maxc == 2) goto done; if (B1 < B2) goto contact2_1; else goto contact2_2; } else { BAR(1,1,2); // use side 2 if (maxc == 2) goto done; if (B1 < B3) goto contact2_1; else goto contact2_3; } } contact2_1: BAR(2,0,1); goto done; contact2_2: BAR(2,1,2); goto done; contact2_3: BAR(2,2,3); goto done; #undef FOO #undef BAR done: for (int i=0; i<ret; i++) { dContactGeom *currContact = CONTACT(contact,i*skip); currContact->g1 = o1; currContact->g2 = o2; currContact->side1 = -1; currContact->side2 = -1; } return ret; }
int dcTriListCollider::dSortedTriCyl ( const dReal* triSideAx0,const dReal* triSideAx1, const dReal* triAx, //const dReal* v0, //const dReal* v1, //const dReal* v2, CDB::TRI* T, dReal dist, dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip ) { VERIFY (dGeomGetClass(o1)== dCylinderClassUser); const dReal *R = dGeomGetRotation(o1); const dReal* p=dGeomGetPosition(o1); dReal radius; dReal hlz; dGeomCylinderGetParams(o1,&radius,&hlz); hlz/=2.f; // find number of contacts requested int maxc = flags & NUMC_MASK; if (maxc < 1) maxc = 1; if (maxc > 3) maxc = 3; // no more than 3 contacts per box allowed dReal signum, outDepth,cos1,sin1; //////////////////////////////////////////////////////////////////////////// //sepparation along tri plane normal;/////////////////////////////////////// //////////////////////////////////////////////////////////////////////////// //cos0=dDOT14(triAx,R+0); cos1=dFabs(dDOT14(triAx,R+1)); //cos2=dDOT14(triAx,R+2); //sin1=_sqrt(cos0*cos0+cos2*cos2); //////////////////////// //another way ////////// cos1=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f sin1=_sqrt(REAL(1.)-cos1*cos1); ////////////////////////////// dReal sidePr=cos1*hlz+sin1*radius; if(dist>0.f) return 0; dReal depth=sidePr-dist; outDepth=depth; signum=-1.f; int code=0; if(depth<0.f) return 0; dVector3 norm; unsigned int ret=0; dVector3 pos; if(code==0){ norm[0]=triAx[0]*signum; norm[1]=triAx[1]*signum; norm[2]=triAx[2]*signum; dReal Q1 = signum*dDOT14(triAx,R+0); dReal Q2 = signum*dDOT14(triAx,R+1); dReal Q3 = signum*dDOT14(triAx,R+2); dReal factor =_sqrt(Q1*Q1+Q3*Q3); dReal C1,C3; dReal centerDepth;//depth in the cirle centre if(factor>0.f) { C1=Q1/factor; C3=Q3/factor; } else { C1=1.f; C3=0.f; } dReal A1 = radius * C1;//cosinus dReal A2 = hlz*Q2; dReal A3 = radius * C3;//sinus if(factor>0.f) centerDepth=outDepth-A1*Q1-A3*Q3; else centerDepth=outDepth; pos[0]=p[0]; pos[1]=p[1]; pos[2]=p[2]; pos[0]+= A2>0 ? hlz*R[1]:-hlz*R[1]; pos[1]+= A2>0 ? hlz*R[5]:-hlz*R[5]; pos[2]+= A2>0 ? hlz*R[9]:-hlz*R[9]; ret=0; contact->pos[0] = pos[0]+A1*R[0]+A3*R[2]; contact->pos[1] = pos[1]+A1*R[4]+A3*R[6]; contact->pos[2] = pos[2]+A1*R[8]+A3*R[10]; { contact->depth = outDepth; ret=1; } if(dFabs(Q2)>M_SQRT1_2){ A1=(-C1*M_COS_PI_3-C3*M_SIN_PI_3)*radius; A3=(-C3*M_COS_PI_3+C1*M_SIN_PI_3)*radius; CONTACT(contact,ret*skip)->pos[0]=pos[0]+A1*R[0]+A3*R[2]; CONTACT(contact,ret*skip)->pos[1]=pos[1]+A1*R[4]+A3*R[6]; CONTACT(contact,ret*skip)->pos[2]=pos[2]+A1*R[8]+A3*R[10]; CONTACT(contact,ret*skip)->depth=centerDepth+Q1*A1+Q3*A3; if(CONTACT(contact,ret*skip)->depth>0.f)++ret; A1=(-C1*M_COS_PI_3+C3*M_SIN_PI_3)*radius; A3=(-C3*M_COS_PI_3-C1*M_SIN_PI_3)*radius; CONTACT(contact,ret*skip)->pos[0]=pos[0]+A1*R[0]+A3*R[2]; CONTACT(contact,ret*skip)->pos[1]=pos[1]+A1*R[4]+A3*R[6]; CONTACT(contact,ret*skip)->pos[2]=pos[2]+A1*R[8]+A3*R[10]; CONTACT(contact,ret*skip)->depth=centerDepth+Q1*A1+Q3*A3; if(CONTACT(contact,ret*skip)->depth>0.f)++ret; } else { CONTACT(contact,ret*skip)->pos[0]=contact->pos[0]-2.f*(A2>0 ? hlz*R[1]:-hlz*R[1]); CONTACT(contact,ret*skip)->pos[1]=contact->pos[1]-2.f*(A2>0 ? hlz*R[5]:-hlz*R[5]); CONTACT(contact,ret*skip)->pos[2]=contact->pos[2]-2.f*(A2>0 ? hlz*R[9]:-hlz*R[9]); CONTACT(contact,ret*skip)->depth=outDepth-Q2*2.f*A2; if(CONTACT(contact,ret*skip)->depth>0.f)++ret; } } if((int)ret>maxc) ret=(unsigned int)maxc; for (unsigned int i=0; i<ret; ++i) { CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o2); CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o1); CONTACT(contact,i*skip)->normal[0] = norm[0]; CONTACT(contact,i*skip)->normal[1] = norm[1]; CONTACT(contact,i*skip)->normal[2] = norm[2]; SURFACE(contact,i*skip)->mode=T->material; } if(ret&&dGeomGetUserData(o1)->callback)dGeomGetUserData(o1)->callback(T,contact); return ret; }
// clip and generate contacts void sTrimeshBoxColliderData::_cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex) { dIASSERT( !(m_iFlags & CONTACTS_UNIMPORTANT) || m_ctContacts < (m_iFlags & NUMC_MASK) ); // Do not call the function if there is no room to store results // if we have edge/edge intersection if (m_iBestAxis > 4 ) { dVector3 vub,vPb,vPa; SET(vPa,m_vHullBoxPos); // calculate point on box edge for( int i=0; i<3; i++) { dVector3 vRotCol; GETCOL(m_mHullBoxRot,i,vRotCol); dReal fSign = dDOT(m_vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f; vPa[0] += fSign * m_vBoxHalfSize[i] * vRotCol[0]; vPa[1] += fSign * m_vBoxHalfSize[i] * vRotCol[1]; vPa[2] += fSign * m_vBoxHalfSize[i] * vRotCol[2]; } int iEdge = (m_iBestAxis-5)%3; // decide which edge is on triangle if ( iEdge == 0 ) { SET(vPb,v0); SET(vub,m_vE0); } else if ( iEdge == 1) { SET(vPb,v2); SET(vub,m_vE1); } else { SET(vPb,v1); SET(vub,m_vE2); } // setup direction parameter for face edge dNormalize3(vub); dReal fParam1, fParam2; // setup direction parameter for box edge dVector3 vua; int col=(m_iBestAxis-5)/3; GETCOL(m_mHullBoxRot,col,vua); // find two closest points on both edges _cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 ); vPa[0] += vua[0]*fParam1; vPa[1] += vua[1]*fParam1; vPa[2] += vua[2]*fParam1; vPb[0] += vub[0]*fParam2; vPb[1] += vub[1]*fParam2; vPb[2] += vub[2]*fParam2; // calculate collision point dVector3 vPntTmp; ADD(vPa,vPb,vPntTmp); vPntTmp[0]*=0.5f; vPntTmp[1]*=0.5f; vPntTmp[2]*=0.5f; // generate contact point between two closest points #if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else dContactGeom* Contact = SAFECONTACT(m_iFlags, m_ContactGeoms, m_ctContacts, m_iStride); Contact->depth = m_fBestDepth; SET(Contact->normal,m_vBestNormal); SET(Contact->pos,vPntTmp); Contact->g1 = Geom1; Contact->g2 = Geom2; Contact->side1 = TriIndex; Contact->side2 = -1; m_ctContacts++; #endif GenerateContact(m_iFlags, m_ContactGeoms, m_iStride, m_Geom1, m_Geom2, TriIndex, vPntTmp, m_vBestNormal, m_fBestDepth, m_ctContacts); // if triangle is the referent face then clip box to triangle face } else if (m_iBestAxis == 1) { dVector3 vNormal2; vNormal2[0]=-m_vBestNormal[0]; vNormal2[1]=-m_vBestNormal[1]; vNormal2[2]=-m_vBestNormal[2]; // vNr is normal in box frame, pointing from triangle to box dMatrix3 mTransposed; mTransposed[0*4+0]=m_mHullBoxRot[0*4+0]; mTransposed[0*4+1]=m_mHullBoxRot[1*4+0]; mTransposed[0*4+2]=m_mHullBoxRot[2*4+0]; mTransposed[1*4+0]=m_mHullBoxRot[0*4+1]; mTransposed[1*4+1]=m_mHullBoxRot[1*4+1]; mTransposed[1*4+2]=m_mHullBoxRot[2*4+1]; mTransposed[2*4+0]=m_mHullBoxRot[0*4+2]; mTransposed[2*4+1]=m_mHullBoxRot[1*4+2]; mTransposed[2*4+2]=m_mHullBoxRot[2*4+2]; dVector3 vNr; vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2]; vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2]; vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2]; dVector3 vAbsNormal; vAbsNormal[0] = dFabs( vNr[0] ); vAbsNormal[1] = dFabs( vNr[1] ); vAbsNormal[2] = dFabs( vNr[2] ); // get closest face from box int iB0, iB1, iB2; if (vAbsNormal[1] > vAbsNormal[0]) { if (vAbsNormal[1] > vAbsNormal[2]) { iB1 = 0; iB0 = 1; iB2 = 2; } else { iB1 = 0; iB2 = 1; iB0 = 2; } } else { if (vAbsNormal[0] > vAbsNormal[2]) { iB0 = 0; iB1 = 1; iB2 = 2; } else { iB1 = 0; iB2 = 1; iB0 = 2; } } // Here find center of box face we are going to project dVector3 vCenter; dVector3 vRotCol; GETCOL(m_mHullBoxRot,iB0,vRotCol); if (vNr[iB0] > 0) { vCenter[0] = m_vHullBoxPos[0] - v0[0] - m_vBoxHalfSize[iB0] * vRotCol[0]; vCenter[1] = m_vHullBoxPos[1] - v0[1] - m_vBoxHalfSize[iB0] * vRotCol[1]; vCenter[2] = m_vHullBoxPos[2] - v0[2] - m_vBoxHalfSize[iB0] * vRotCol[2]; } else { vCenter[0] = m_vHullBoxPos[0] - v0[0] + m_vBoxHalfSize[iB0] * vRotCol[0]; vCenter[1] = m_vHullBoxPos[1] - v0[1] + m_vBoxHalfSize[iB0] * vRotCol[1]; vCenter[2] = m_vHullBoxPos[2] - v0[2] + m_vBoxHalfSize[iB0] * vRotCol[2]; } // Here find 4 corner points of box dVector3 avPoints[4]; dVector3 vRotCol2; GETCOL(m_mHullBoxRot,iB1,vRotCol); GETCOL(m_mHullBoxRot,iB2,vRotCol2); for(int x=0;x<3;x++) { avPoints[0][x] = vCenter[x] + (m_vBoxHalfSize[iB1] * vRotCol[x]) - (m_vBoxHalfSize[iB2] * vRotCol2[x]); avPoints[1][x] = vCenter[x] - (m_vBoxHalfSize[iB1] * vRotCol[x]) - (m_vBoxHalfSize[iB2] * vRotCol2[x]); avPoints[2][x] = vCenter[x] - (m_vBoxHalfSize[iB1] * vRotCol[x]) + (m_vBoxHalfSize[iB2] * vRotCol2[x]); avPoints[3][x] = vCenter[x] + (m_vBoxHalfSize[iB1] * vRotCol[x]) + (m_vBoxHalfSize[iB2] * vRotCol2[x]); } // clip Box face with 4 planes of triangle (1 face plane, 3 egde planes) dVector3 avTempArray1[9]; dVector3 avTempArray2[9]; dVector4 plPlane; int iTempCnt1=0; int iTempCnt2=0; // zeroify vectors - necessary? for(int i=0; i<9; i++) { avTempArray1[i][0]=0; avTempArray1[i][1]=0; avTempArray1[i][2]=0; avTempArray2[i][0]=0; avTempArray2[i][1]=0; avTempArray2[i][2]=0; } // Normal plane dVector3 vTemp; vTemp[0]=-m_vN[0]; vTemp[1]=-m_vN[1]; vTemp[2]=-m_vN[2]; dNormalize3(vTemp); CONSTRUCTPLANE(plPlane,vTemp,0); _cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane ); // Plane p0 dVector3 vTemp2; SUBTRACT(v1,v0,vTemp2); dCROSS(vTemp,=,m_vN,vTemp2); dNormalize3(vTemp); CONSTRUCTPLANE(plPlane,vTemp,0); _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); // Plane p1 SUBTRACT(v2,v1,vTemp2); dCROSS(vTemp,=,m_vN,vTemp2); dNormalize3(vTemp); SUBTRACT(v0,v2,vTemp2); CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp)); _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); // Plane p2 SUBTRACT(v0,v2,vTemp2); dCROSS(vTemp,=,m_vN,vTemp2); dNormalize3(vTemp); CONSTRUCTPLANE(plPlane,vTemp,0); _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); // END of clipping polygons // for each generated contact point for ( int i=0; i<iTempCnt2; i++ ) { // calculate depth dReal fTempDepth = dDOT(vNormal2,avTempArray2[i]); // clamp depth to zero if (fTempDepth > 0) { fTempDepth = 0; } dVector3 vPntTmp; ADD(avTempArray2[i],v0,vPntTmp); #if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else dContactGeom* Contact = SAFECONTACT(m_iFlags, m_ContactGeoms, m_ctContacts, m_iStride); Contact->depth = -fTempDepth; SET(Contact->normal,m_vBestNormal); SET(Contact->pos,vPntTmp); Contact->g1 = Geom1; Contact->g2 = Geom2; Contact->side1 = TriIndex; Contact->side2 = -1; m_ctContacts++; #endif GenerateContact(m_iFlags, m_ContactGeoms, m_iStride, m_Geom1, m_Geom2, TriIndex, vPntTmp, m_vBestNormal, -fTempDepth, m_ctContacts); if ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } //dAASSERT(m_ctContacts>0); // if box face is the referent face, then clip triangle on box face } else { // 2 <= if iBestAxis <= 4
static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body, dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b, dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, dxQuickStepParameters *qs) { const int num_iterations = qs->num_iterations; const dReal sor_w = qs->w; // SOR over-relaxation parameter int i,j; #ifdef WARM_STARTING // for warm starting, this seems to be necessary to prevent // jerkiness in motor-driven joints. i have no idea why this works. for (i=0; i<m; i++) lambda[i] *= 0.9; #else dSetZero (lambda,m); #endif // the lambda computed at the previous iteration. // this is used to measure error for when we are reordering the indexes. dRealAllocaArray (last_lambda,m); // a copy of the 'hi' vector in case findex[] is being used dRealAllocaArray (hicopy,m); memcpy (hicopy,hi,m*sizeof(dReal)); // precompute iMJ = inv(M)*J' dRealAllocaArray (iMJ,m*12); compute_invM_JT (m,J,iMJ,jb,body,invI); // compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc // as we change lambda. #ifdef WARM_STARTING multiply_invM_JT (m,nb,iMJ,jb,lambda,fc); #else dSetZero (fc,nb*6); #endif // precompute 1 / diagonals of A dRealAllocaArray (Ad,m); dRealPtr iMJ_ptr = iMJ; dRealMutablePtr J_ptr = J; for (i=0; i<m; i++) { dReal sum = 0; for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j]; if (jb[i*2+1] >= 0) { for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; } iMJ_ptr += 12; J_ptr += 12; Ad[i] = sor_w / (sum + cfm[i]); } // scale J and b by Ad J_ptr = J; for (i=0; i<m; i++) { for (j=0; j<12; j++) { J_ptr[0] *= Ad[i]; J_ptr++; } b[i] *= Ad[i]; } // scale Ad by CFM for (i=0; i<m; i++) Ad[i] *= cfm[i]; // order to solve constraint rows in IndexError *order = (IndexError*) alloca (m*sizeof(IndexError)); #ifndef REORDER_CONSTRAINTS // make sure constraints with findex < 0 come first. j=0; for (i=0; i<m; i++) if (findex[i] < 0) order[j++].index = i; for (i=0; i<m; i++) if (findex[i] >= 0) order[j++].index = i; dIASSERT (j==m); #endif for (int iteration=0; iteration < num_iterations; iteration++) { #ifdef REORDER_CONSTRAINTS // constraints with findex < 0 always come first. if (iteration < 2) { // for the first two iterations, solve the constraints in // the given order for (i=0; i<m; i++) { order[i].error = i; order[i].findex = findex[i]; order[i].index = i; } } else { // sort the constraints so that the ones converging slowest // get solved last. use the absolute (not relative) error. for (i=0; i<m; i++) { dReal v1 = dFabs (lambda[i]); dReal v2 = dFabs (last_lambda[i]); dReal max = (v1 > v2) ? v1 : v2; if (max > 0) { //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max; order[i].error = dFabs(lambda[i]-last_lambda[i]); } else { order[i].error = dInfinity; } order[i].findex = findex[i]; order[i].index = i; } } qsort (order,m,sizeof(IndexError),&compare_index_error); #endif #ifdef RANDOMLY_REORDER_CONSTRAINTS if ((iteration & 7) == 0) { for (i=1; i<m; ++i) { IndexError tmp = order[i]; int swapi = dRandInt(i+1); order[i] = order[swapi]; order[swapi] = tmp; } } #endif //@@@ potential optimization: swap lambda and last_lambda pointers rather // than copying the data. we must make sure lambda is properly // returned to the caller memcpy (last_lambda,lambda,m*sizeof(dReal)); for (int i=0; i<m; i++) { // @@@ potential optimization: we could pre-sort J and iMJ, thereby // linearizing access to those arrays. hmmm, this does not seem // like a win, but we should think carefully about our memory // access pattern. int index = order[i].index; J_ptr = J + index*12; iMJ_ptr = iMJ + index*12; // set the limits for this constraint. note that 'hicopy' is used. // this is the place where the QuickStep method differs from the // direct LCP solving method, since that method only performs this // limit adjustment once per time step, whereas this method performs // once per iteration per constraint row. // the constraints are ordered so that all lambda[] values needed have // already been computed. if (findex[index] >= 0) { hi[index] = dFabs (hicopy[index] * lambda[findex[index]]); lo[index] = -hi[index]; } int b1 = jb[index*2]; int b2 = jb[index*2+1]; dReal delta = b[index] - lambda[index]*Ad[index]; dRealMutablePtr fc_ptr = fc + 6*b1; // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5]; // @@@ potential optimization: handle 1-body constraints in a separate // loop to avoid the cost of test & jump? if (b2 >= 0) { fc_ptr = fc + 6*b2; delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] + fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] + fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11]; } // compute lambda and clamp it to [lo,hi]. // @@@ potential optimization: does SSE have clamping instructions // to save test+jump penalties here? dReal new_lambda = lambda[index] + delta; if (new_lambda < lo[index]) { delta = lo[index]-lambda[index]; lambda[index] = lo[index]; } else if (new_lambda > hi[index]) { delta = hi[index]-lambda[index]; lambda[index] = hi[index]; } else { lambda[index] = new_lambda; } //@@@ a trick that may or may not help //dReal ramp = (1-((dReal)(iteration+1)/(dReal)num_iterations)); //delta *= ramp; // update fc. // @@@ potential optimization: SIMD for this and the b2 >= 0 case fc_ptr = fc + 6*b1; fc_ptr[0] += delta * iMJ_ptr[0]; fc_ptr[1] += delta * iMJ_ptr[1]; fc_ptr[2] += delta * iMJ_ptr[2]; fc_ptr[3] += delta * iMJ_ptr[3]; fc_ptr[4] += delta * iMJ_ptr[4]; fc_ptr[5] += delta * iMJ_ptr[5]; // @@@ potential optimization: handle 1-body constraints in a separate // loop to avoid the cost of test & jump? if (b2 >= 0) { fc_ptr = fc + 6*b2; fc_ptr[0] += delta * iMJ_ptr[6]; fc_ptr[1] += delta * iMJ_ptr[7]; fc_ptr[2] += delta * iMJ_ptr[8]; fc_ptr[3] += delta * iMJ_ptr[9]; fc_ptr[4] += delta * iMJ_ptr[10]; fc_ptr[5] += delta * iMJ_ptr[11]; } } } }
bool sTrimeshBoxColliderData::_cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { // reset best axis m_iBestAxis = 0; m_iExitAxis = -1; m_fBestDepth = MAXVALUE; // calculate edges SUBTRACT(v1,v0,m_vE0); SUBTRACT(v2,v0,m_vE1); SUBTRACT(m_vE1,m_vE0,m_vE2); // calculate poly normal dCROSS(m_vN,=,m_vE0,m_vE1); // calculate length of face normal dReal fNLen = LENGTHOF(m_vN); // Even though all triangles might be initially valid, // a triangle may degenerate into a segment after applying // space transformation. if (!fNLen) { return false; } // extract box axes as vectors dVector3 vA0,vA1,vA2; GETCOL(m_mHullBoxRot,0,vA0); GETCOL(m_mHullBoxRot,1,vA1); GETCOL(m_mHullBoxRot,2,vA2); // box halfsizes dReal fa0 = m_vBoxHalfSize[0]; dReal fa1 = m_vBoxHalfSize[1]; dReal fa2 = m_vBoxHalfSize[2]; // calculate relative position between box and triangle dVector3 vD; SUBTRACT(v0,m_vHullBoxPos,vD); dVector3 vL; dReal fp0, fp1, fp2, fR, fD; // Test separating axes for intersection // ************************************************ // Axis 1 - Triangle Normal SET(vL,m_vN); fp0 = dDOT(vL,vD); fp1 = fp0; fp2 = fp0; fR=fa0*dFabs( dDOT(m_vN,vA0) ) + fa1 * dFabs( dDOT(m_vN,vA1) ) + fa2 * dFabs( dDOT(m_vN,vA2) ); if (!_cldTestNormal(fp0, fR, vL, 1)) { m_iExitAxis=1; return false; } // ************************************************ // Test Faces // ************************************************ // Axis 2 - Box X-Axis SET(vL,vA0); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0 + dDOT(vA0,m_vE0); fp2 = fp0 + dDOT(vA0,m_vE1); fR = fa0; if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 2)) { m_iExitAxis=2; return false; } // ************************************************ // ************************************************ // Axis 3 - Box Y-Axis SET(vL,vA1); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0 + dDOT(vA1,m_vE0); fp2 = fp0 + dDOT(vA1,m_vE1); fR = fa1; if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 3)) { m_iExitAxis=3; return false; } // ************************************************ // ************************************************ // Axis 4 - Box Z-Axis SET(vL,vA2); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0 + dDOT(vA2,m_vE0); fp2 = fp0 + dDOT(vA2,m_vE1); fR = fa2; if (!_cldTestFace(fp0, fp1, fp2, fR, fD, vL, 4)) { m_iExitAxis=4; return false; } // ************************************************ // Test Edges // ************************************************ // Axis 5 - Box X-Axis cross Edge0 dCROSS(vL,=,vA0,m_vE0); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0; fp2 = fp0 + dDOT(vA0,m_vN); fR = fa1 * dFabs(dDOT(vA2,m_vE0)) + fa2 * dFabs(dDOT(vA1,m_vE0)); if (!_cldTestEdge(fp1, fp2, fR, fD, vL, 5)) { m_iExitAxis=5; return false; } // ************************************************ // ************************************************ // Axis 6 - Box X-Axis cross Edge1 dCROSS(vL,=,vA0,m_vE1); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0 - dDOT(vA0,m_vN); fp2 = fp0; fR = fa1 * dFabs(dDOT(vA2,m_vE1)) + fa2 * dFabs(dDOT(vA1,m_vE1)); if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 6)) { m_iExitAxis=6; return false; } // ************************************************ // ************************************************ // Axis 7 - Box X-Axis cross Edge2 dCROSS(vL,=,vA0,m_vE2); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0 - dDOT(vA0,m_vN); fp2 = fp0 - dDOT(vA0,m_vN); fR = fa1 * dFabs(dDOT(vA2,m_vE2)) + fa2 * dFabs(dDOT(vA1,m_vE2)); if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 7)) { m_iExitAxis=7; return false; } // ************************************************ // ************************************************ // Axis 8 - Box Y-Axis cross Edge0 dCROSS(vL,=,vA1,m_vE0); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0; fp2 = fp0 + dDOT(vA1,m_vN); fR = fa0 * dFabs(dDOT(vA2,m_vE0)) + fa2 * dFabs(dDOT(vA0,m_vE0)); if (!_cldTestEdge(fp0, fp2, fR, fD, vL, 8)) { m_iExitAxis=8; return false; } // ************************************************ // ************************************************ // Axis 9 - Box Y-Axis cross Edge1 dCROSS(vL,=,vA1,m_vE1); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0 - dDOT(vA1,m_vN); fp2 = fp0; fR = fa0 * dFabs(dDOT(vA2,m_vE1)) + fa2 * dFabs(dDOT(vA0,m_vE1)); if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 9)) { m_iExitAxis=9; return false; } // ************************************************ // ************************************************ // Axis 10 - Box Y-Axis cross Edge2 dCROSS(vL,=,vA1,m_vE2); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0 - dDOT(vA1,m_vN); fp2 = fp0 - dDOT(vA1,m_vN); fR = fa0 * dFabs(dDOT(vA2,m_vE2)) + fa2 * dFabs(dDOT(vA0,m_vE2)); if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 10)) { m_iExitAxis=10; return false; } // ************************************************ // ************************************************ // Axis 11 - Box Z-Axis cross Edge0 dCROSS(vL,=,vA2,m_vE0); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0; fp2 = fp0 + dDOT(vA2,m_vN); fR = fa0 * dFabs(dDOT(vA1,m_vE0)) + fa1 * dFabs(dDOT(vA0,m_vE0)); if (!_cldTestEdge(fp0, fp2, fR, fD, vL, 11)) { m_iExitAxis=11; return false; } // ************************************************ // ************************************************ // Axis 12 - Box Z-Axis cross Edge1 dCROSS(vL,=,vA2,m_vE1); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0 - dDOT(vA2,m_vN); fp2 = fp0; fR = fa0 * dFabs(dDOT(vA1,m_vE1)) + fa1 * dFabs(dDOT(vA0,m_vE1)); if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 12)) { m_iExitAxis=12; return false; } // ************************************************ // ************************************************ // Axis 13 - Box Z-Axis cross Edge2 dCROSS(vL,=,vA2,m_vE2); fD = dDOT(vL,m_vN)/fNLen; fp0 = dDOT(vL,vD); fp1 = fp0 - dDOT(vA2,m_vN); fp2 = fp0 - dDOT(vA2,m_vN); fR = fa0 * dFabs(dDOT(vA1,m_vE2)) + fa1 * dFabs(dDOT(vA0,m_vE2)); if (!_cldTestEdge(fp0, fp1, fR, fD, vL, 13)) { m_iExitAxis=13; return false; } // ************************************************ return true; }
void dxJointScrew::getInfo2( dxJoint::Info2 *info ) { // Added by OSRF // // Screw Constraint Overview // // make 5 constraint rows. // given screw axis, first create two orthogonal axis p and q. // row 1: linear constraint along p // row 2: linear constraint along q // row 3: screw constraint about user specified axis // row 4: rotational constraint about p // row 5: rotational constraint about q // Added by OSRF // If joint values of erp and cfm are negative, then ignore them. // info->erp, info->cfm already have the global values from quickstep if (this->erp >= 0) info->erp = erp; if (this->cfm >= 0) { info->cfm[0] = cfm; info->cfm[1] = cfm; info->cfm[2] = cfm; info->cfm[3] = cfm; info->cfm[4] = cfm; } // constraint rows 1 to 3 { // pull out pos and R for both bodies. also get the `connection' // vector pos2-pos1. dReal *pos1, *pos2, *R1, *R2; dVector3 cgdiff; cgdiff[0] = cgdiff[1] = cgdiff[2] = 0; pos1 = node[0].body->posr.pos; R1 = node[0].body->posr.R; if ( node[1].body ) { pos2 = node[1].body->posr.pos; R2 = node[1].body->posr.R; for (int i = 0; i < 3; ++i ) { // store distance between cg's in cgdiff cgdiff[i] = pos2[i] - pos1[i]; } } else { pos2 = 0; R2 = 0; } // compute error for screw due to drift dReal lin_disp; // linear displacement dReal lin_err; // linear displacement { // get linear disp for screw // get axis1 in global coordinates dVector3 ax1, q; dMultiply0_331 ( ax1, node[0].body->posr.R, axis1 ); if ( node[1].body ) { // get body2 + offset point in global coordinates dMultiply0_331 ( q, node[1].body->posr.R, offset ); //printf("debug offset q[%f %f %f] p0[%f %f %f] p1[%f %f %f] \t", // q[0],q[1],q[2], // node[0].body->posr.pos[0], node[0].body->posr.pos[1], // node[0].body->posr.pos[2], // node[1].body->posr.pos[0], node[1].body->posr.pos[1], // node[1].body->posr.pos[2]); for ( int ii = 0; ii < 3; ++ii ) q[ii] = node[0].body->posr.pos[ii] - q[ii] - node[1].body->posr.pos[ii]; } else { q[0] = node[0].body->posr.pos[0] - offset[0]; q[1] = node[0].body->posr.pos[1] - offset[1]; q[2] = node[0].body->posr.pos[2] - offset[2]; } lin_disp = dCalcVectorDot3 ( ax1, q ); // linear error should be length scaled, BUT if (dFabs(thread_pitch) > 1.0) { // constraint is written in length scale, so // linear error is length scaled. lin_err = -(lin_disp-cumulative_angle/thread_pitch); } else { // here the entire constraint equation, including lin_err // is multiplied by thread_pitch for |thread_pitch| less than 1.0 // for added numerical stability, lin_err = -(thread_pitch*lin_disp-cumulative_angle); } // printf("lin disp: %f lin err: %f\n", lin_disp, lin_err); } int s0 = 0 * info->rowskip; int s1 = 1 * info->rowskip; int s2 = 2 * info->rowskip; // remaining two rows. we want: vel2 = vel1 + w1 x cgdiff ... but this would // result in three equations, so we project along the planespace vectors // so that sliding along the slider axis is disregarded. for symmetry we // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. // ax1 is axis1 converted to body1 frame dVector3 ax1; dMultiply0_331 ( ax1, R1, axis1 ); // p and q are vectors perpendicular to ax1 in body1 frame dVector3 p, q; dPlaneSpace ( ax1, p, q ); // linear constraints for the hinge joint // perpendicular to the sliding axis direction. for (int i = 0; i < 3; ++i ) info->J1l[s0+i] = p[i]; for (int i = 0; i < 3; ++i ) info->J1l[s1+i] = q[i]; // if p and q do not pass through body CG's, // we need to add angular constraints to balance out the forces // from these linear constraints. See below: // a1 and a2 are axis vectors in the body frame // (whereas anchor1 and anchor2 are in world frame). // anchor1 is the vector from CG to joint anchor in world frame. dVector3 a1, a2; dMultiply0_331( a1, R1, anchor1 ); // tmpp is a vector perpendicular to a1 and p in body frame, // it is the direction of the angular constraint that will // cancel out moment generated by linear constraint p // if p does not pass through CG. { dVector3 tmpp; dCalcVectorCross3(tmpp, p, a1); for (int i = 0; i < 3; ++i ) info->J1a[s0+i] = -tmpp[i]; } // tmpq is similar to tmpp, but for q. { dVector3 tmpq; dCalcVectorCross3(tmpq, q, a1); for (int i = 0; i < 3; ++i ) info->J1a[s1+i] = -tmpq[i]; } // screw constraint: // now constrain the sliding axis by rotation of the other body if (dFabs(thread_pitch) > 1.0) { for (int i = 0; i < 3; ++i ) info->J1l[s2+i] = ax1[i]; for (int i = 0; i < 3; ++i ) info->J1a[s2+i] = -ax1[i]/thread_pitch; } else { // here the entire constraint equation, including lin_err // is multiplied by thread_pitch for |thread_pitch| less than 1.0 // for added numerical stability, for (int i = 0; i < 3; ++i ) info->J1l[s2+i] = ax1[i]*thread_pitch; for (int i = 0; i < 3; ++i ) info->J1a[s2+i] = -ax1[i]; } // repeat above for child body if one exists if ( node[1].body ) { // linear constraints for s0 and s1 for (int i = 0; i < 3; ++i ) info->J2l[s0+i] = -p[i]; for (int i = 0; i < 3; ++i ) info->J2l[s1+i] = -q[i]; // angular compensation if p and q do not pass through CG dMultiply0_331( a2, R2, anchor2 ); dVector3 tmpp; dCalcVectorCross3(tmpp, p, a2); for (int i = 0; i < 3; ++i ) info->J2a[s0+i] = tmpp[i]; dVector3 tmpq; dCalcVectorCross3(tmpq, q, a2); for (int i = 0; i < 3; ++i ) info->J2a[s1+i] = tmpq[i]; // screw constraint: // constrain the sliding axis by rotation of the other body if (dFabs(thread_pitch) > 1.0) { for (int i = 0; i < 3; ++i ) info->J2a[s2+i] = ax1[i]/thread_pitch; for (int i = 0; i < 3; ++i ) info->J2l[s2+i] = -ax1[i]; } else { // here the entire constraint equation, including lin_err // is multiplied by thread_pitch for |thread_pitch| less than 1.0 // for added numerical stability, for (int i = 0; i < 3; ++i ) info->J2a[s2+i] = ax1[i]; for (int i = 0; i < 3; ++i ) info->J2l[s2+i] = -ax1[i]*thread_pitch; } } // debug // printf ("anchor1 %f %f %f\n", anchor1[0], anchor1[1], anchor1[2]); // printf ("a1 %f %f %f\n", a1[0], a1[1], a1[2]); // printf ("ax1 %f %f %f\n", ax1[0], ax1[1], ax1[2]); // printf ("tmpp %f %f %f\n", tmpp[0], tmpp[1], tmpp[2]); // printf ("p %f %f %f\n", p[0], p[1], p[2]); // printf ("q %f %f %f\n", q[0], q[1], q[2]); // printf ("J1a[s0] %f %f %f\n", info->J1a[s0+0], info->J1a[s0+1], // info->J1a[s0+2]); // printf ("J1a[s1] %f %f %f\n", info->J1a[s1+0], info->J1a[s1+1], // info->J1a[s1+2]); // info->J1a[s0+0] = 1; // info->J1a[s0+1] = 0; // info->J1a[s0+2] = 0; // info->J1a[s1+0] = -1; // info->J1a[s1+1] = 0; // info->J1a[s1+2] = 0; // printf("screw err lin[%f], ang[%f], diff[%f] tp[%f]\n", // thread_pitch*lin_disp, cumulative_angle, lin_err, thread_pitch); // compute last two elements of right hand side. we want to align the offset // point (in body 2's frame) with the center of body 1. dReal k = info->fps * info->erp; if ( node[1].body ) { // dVector3 ofs; // offset point in global coordinates // dMultiply0_331 ( ofs, R2, offset ); // for (int i = 0; i < 3; ++i ) cgdiff[i] += ofs[i]; // error between body anchors dVector3 error12; for (int i = 0; i < 3; ++i) error12[i] = a2[i] + node[1].body->posr.pos[i] - a1[i] - node[0].body->posr.pos[i]; // error in the p direction is error12 dot p info->c[0] = k * (dCalcVectorDot3(error12, p)); // error in the q direction is error12 dot p info->c[1] = k * (dCalcVectorDot3(error12, q)); // interpenetration error for screw constraint info->c[2] = k * lin_err; } else { // debug // printf ("anchor1 %f %f %f\n", anchor1[0], anchor1[1], anchor1[2]); // printf ("anchor2 %f %f %f\n", anchor2[0], anchor2[1], anchor2[2]); // printf ("a1 %f %f %f\n", a1[0], a1[1], a1[2]); // printf ("p1 %f %f %f\n", // node[0].body->posr.pos[0], // node[0].body->posr.pos[1], // node[0].body->posr.pos[2]); // error of body's anchor dVector3 error1; for (int i = 0; i < 3; ++i) error1[i] = anchor2[i] - a1[i] - node[0].body->posr.pos[i]; // printf ("error1 %f %f %f\n", error1[0], error1[1], error1[2]); // error in the p direction is error1 dot p info->c[0] = k * (dCalcVectorDot3(error1, p)); // error in the q direction info->c[1] = k * (dCalcVectorDot3(error1, q)); // interpenetration error for screw constraint info->c[2] = k * lin_err; if ( flags & dJOINT_REVERSE ) for (int i = 0; i < 3; ++i ) ax1[i] = -ax1[i]; } // uncommnet to enforce slider joint limit // limot.addLimot ( this, info, 5, ax1, 0 ); } // constraint rows 4 and 5 { // set the two hinge rows. the screw axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the hinge axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 // are the angular velocity vectors of the two bodies. dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body dVector3 p, q; // plane space vectors for ax1 dMultiply0_331( ax1, node[0].body->posr.R, axis1 ); dPlaneSpace( ax1, p, q ); int s3 = 3 * info->rowskip; int s4 = 4 * info->rowskip; info->J1a[s3+0] = p[0]; info->J1a[s3+1] = p[1]; info->J1a[s3+2] = p[2]; info->J1a[s4+0] = q[0]; info->J1a[s4+1] = q[1]; info->J1a[s4+2] = q[2]; if ( node[1].body ) { info->J2a[s3+0] = -p[0]; info->J2a[s3+1] = -p[1]; info->J2a[s3+2] = -p[2]; info->J2a[s4+0] = -q[0]; info->J2a[s4+1] = -q[1]; info->J2a[s4+2] = -q[2]; } // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the screw back into alignment. // if ax1,ax2 are the unit length screw axes as computed from body1 and // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). // if `theta' is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. dVector3 ax2, b; if ( node[1].body ) { dMultiply0_331( ax2, node[1].body->posr.R, axis2 ); } else { ax2[0] = axis2[0]; ax2[1] = axis2[1]; ax2[2] = axis2[2]; } dCalcVectorCross3( b, ax1, ax2 ); dReal k = info->fps * info->erp; info->c[3] = k * dCalcVectorDot3( b, p ); info->c[4] = k * dCalcVectorDot3( b, q ); // enforcing rotation joint limit limot.addLimot( this, info, 5, ax1, 1 ); } }