//plane plane
bool FindIntersectionPlanePlane(const dReal Plane0[4], const dReal Plane1[4],
dVector3 LinePos,dVector3 LineDir)
{
// If Cross(N0,N1) is zero, then either planes are parallel and separated
// or the same plane. In both cases, 'false' is returned. Otherwise,
// the intersection line is
//
// L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1
//
// for some coefficients c0 and c1 and for t any real number (the line
// parameter). Taking dot products with the normals,
//
// d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1)
// d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1)
//
// which are two equations in two unknowns. The solution is
//
// c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det
// c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det
//
// where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2.
/*
Real fN00 = rkPlane0.Normal().SquaredLength();
Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal());
Real fN11 = rkPlane1.Normal().SquaredLength();
Real fDet = fN00*fN11 - fN01*fN01;
if ( Math::FAbs(fDet) < gs_fEpsilon )
return false;
Real fInvDet = 1.0f/fDet;
Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet;
Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet;
rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal());
rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal();
return true;
*/
dReal fN00 = dLENGTHSQUARED(Plane0);
dReal fN01 = dDOT(Plane0,Plane1);
dReal fN11 = dLENGTHSQUARED(Plane1);
dReal fDet = fN00*fN11 - fN01*fN01;
if ( fabs(fDet) < fEPSILON)
return false;
dReal fInvDet = 1.0f/fDet;
dReal fC0 = (fN11*Plane0[3] - fN01*Plane1[3])*fInvDet;
dReal fC1 = (fN00*Plane1[3] - fN01*Plane0[3])*fInvDet;
dCROSS(LineDir,=,Plane0,Plane1);
dNormalize3(LineDir);
dVector3 Temp0,Temp1;
dOPC(Temp0,*,Plane0,fC0);
dOPC(Temp1,*,Plane1,fC1);
dOP(LinePos,+,Temp0,Temp1);
return true;
}
void dJointPlanarSetPlaneNormal(dJointID joint, dVector3 planeNormal) {
dUASSERT( joint, "bad joint argument" );
checktype( joint, Plane2D );
dxPlanarJoint* planarJoint = (dxPlanarJoint*) joint;
dUASSERT(dLENGTHSQUARED(planeNormal) > 0.000001, "plane normal cannot have zero length");
dCopyVector3(planarJoint->planeNormal, planeNormal);
dNormalize3(planarJoint->planeNormal);
planarJoint->updatePlane();
}
/*
* This takes what is supposed to be a rotation matrix,
* and make sure it is correct.
* Note: this operates on rows, not columns, because for rotations
* both ways give equivalent results.
*/
void dOrthogonalizeR(dMatrix3 m)
{
dReal n0 = dLENGTHSQUARED(m);
if (n0 != 1)
dSafeNormalize3(m);
// project row[0] on row[1], should be zero
dReal proj = dDOT(m, m+4);
if (proj != 0) {
// Gram-Schmidt step on row[1]
m[4] -= proj * m[0];
m[5] -= proj * m[1];
m[6] -= proj * m[2];
}
dReal n1 = dLENGTHSQUARED(m+4);
if (n1 != 1)
dSafeNormalize3(m+4);
/* just overwrite row[2], this makes sure the matrix is not
a reflection */
dCROSS(m+8, =, m, m+4);
m[3] = m[4+3] = m[8+3] = 0;
}