// 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 );
}
}
