void dLCP::transfer_i_from_N_to_C (int i)
{
int j;
if (nC > 0) {
dReal *aptr = AROW(i);
# ifdef NUB_OPTIMIZATIONS
// if nub>0, initial part of aptr unpermuted
for (j=0; j<nub; j++) Dell[j] = aptr[j];
for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]];
# else
for (j=0; j<nC; j++) Dell[j] = aptr[C[j]];
# endif
dSolveL1 (L,Dell,nC,nskip);
for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j];
for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
}
else {
d[0] = dRecip (AROW(i)[i]);
}
swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
C[nC] = nC;
nN--;
nC++;
// @@@ TO DO LATER
// if we just finish here then we'll go back and re-solve for
// delta_x. but actually we can be more efficient and incrementally
// update delta_x here. but if we do this, we wont have ell and Dell
// to use in updating the factorization later.
# ifdef DEBUG_LCP
checkFactorization (A,L,d,nC,C,nskip);
# endif
}
void dLCP::transfer_i_to_C (int i)
{
{
if (m_nC > 0) {
// ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
{
const int nC = m_nC;
dReal *const Ltgt = m_L + nC*m_nskip, *ell = m_ell;
for (int j=0; j<nC; ++j) Ltgt[j] = ell[j];
}
const int nC = m_nC;
m_d[nC] = dRecip (AROW(i)[i] - dDot(m_ell,m_Dell,nC));
}
else {
m_d[0] = dRecip (AROW(i)[i]);
}
swapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);
const int nC = m_nC;
m_C[nC] = nC;
m_nC = nC + 1; // nC value is outdated after this line
}
# ifdef DEBUG_LCP
checkFactorization (m_A,m_L,m_d,m_nC,m_C,m_nskip);
if (i < (m_n-1)) checkPermutations (i+1,m_n,m_nC,m_nN,m_p,m_C);
# endif
}
void dLCP::transfer_i_from_N_to_C (int i)
{
{
if (m_nC > 0) {
{
dReal *const aptr = AROW(i);
dReal *Dell = m_Dell;
const int *C = m_C;
# ifdef NUB_OPTIMIZATIONS
// if nub>0, initial part of aptr unpermuted
const int nub = m_nub;
int j=0;
for ( ; j<nub; ++j) Dell[j] = aptr[j];
const int nC = m_nC;
for ( ; j<nC; ++j) Dell[j] = aptr[C[j]];
# else
const int nC = m_nC;
for (int j=0; j<nC; ++j) Dell[j] = aptr[C[j]];
# endif
}
dSolveL1 (m_L,m_Dell,m_nC,m_nskip);
{
const int nC = m_nC;
dReal *const Ltgt = m_L + nC*m_nskip;
dReal *ell = m_ell, *Dell = m_Dell, *d = m_d;
for (int j=0; j<nC; ++j) Ltgt[j] = ell[j] = Dell[j] * d[j];
}
const int nC = m_nC;
dReal Aii_dDot = AROW(i)[i] - dDot(m_ell, m_Dell, nC);
if(dFabs(Aii_dDot) < 1e-16) {
Aii_dDot += 1e-6;
}
m_d[nC] = dRecip (Aii_dDot);
}
else {
if(dFabs(AROW(i)[i]) < 1e-16) {
AROW(i)[i] += 1e-6;
}
m_d[0] = dRecip (AROW(i)[i]);
}
swapProblem (m_A,m_x,m_b,m_w,m_lo,m_hi,m_p,m_state,m_findex,m_n,m_nC,i,m_nskip,1);
const int nC = m_nC;
m_C[nC] = nC;
m_nN--;
m_nC = nC + 1; // nC value is outdated after this line
}
// @@@ TO DO LATER
// if we just finish here then we'll go back and re-solve for
// delta_x. but actually we can be more efficient and incrementally
// update delta_x here. but if we do this, we wont have ell and Dell
// to use in updating the factorization later.
# ifdef DEBUG_LCP
checkFactorization (m_A,m_L,m_d,m_nC,m_C,m_nskip);
# endif
}
void dQfromR (dQuaternion q, const dMatrix3 R)
{
dAASSERT (q && R);
dReal tr(0), s(0);
tr = _R(0,0) + _R(1,1) + _R(2,2);
if (tr >= 0)
{
s = dSqrt (tr + 1);
q[0] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[1] = (_R(2,1) - _R(1,2)) * s;
q[2] = (_R(0,2) - _R(2,0)) * s;
q[3] = (_R(1,0) - _R(0,1)) * s;
}
else {
// find the largest diagonal element and jump to the appropriate case
if (_R(1,1) > _R(0,0)) {
if (_R(2,2) > _R(1,1)) goto case_2;
goto case_1;
}
if (_R(2,2) > _R(0,0)) goto case_2;
goto case_0;
case_0:
s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1);
q[1] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[2] = (_R(0,1) + _R(1,0)) * s;
q[3] = (_R(2,0) + _R(0,2)) * s;
q[0] = (_R(2,1) - _R(1,2)) * s;
return;
case_1:
s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1);
q[2] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[3] = (_R(1,2) + _R(2,1)) * s;
q[1] = (_R(0,1) + _R(1,0)) * s;
q[0] = (_R(0,2) - _R(2,0)) * s;
return;
case_2:
s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1);
q[3] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[1] = (_R(2,0) + _R(0,2)) * s;
q[2] = (_R(1,2) + _R(2,1)) * s;
q[0] = (_R(1,0) - _R(0,1)) * s;
return;
}
}
EXPORT_C void dQfromR (dQuaternion q, const dMatrix3 R)
{
dReal tr,s;
tr = _R(0,0) + _R(1,1) + _R(2,2);
if (tr >= 0) {
s = dSqrt (tr + REAL(1.0));
q[0] = dMUL(REAL(0.5),s);
s = dMUL(REAL(0.5),dRecip(s));
q[1] = dMUL((_R(2,1) - _R(1,2)),s);
q[2] = dMUL((_R(0,2) - _R(2,0)),s);
q[3] = dMUL((_R(1,0) - _R(0,1)),s);
}
else {
// find the largest diagonal element and jump to the appropriate case
if (_R(1,1) > _R(0,0)) {
if (_R(2,2) > _R(1,1)) goto case_2;
goto case_1;
}
if (_R(2,2) > _R(0,0)) goto case_2;
goto case_0;
case_0:
s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + REAL(1.0));
q[1] = dMUL(REAL(0.5),s);
s = dMUL(REAL(0.5),dRecip(s));
q[2] = dMUL((_R(0,1) + _R(1,0)),s);
q[3] = dMUL((_R(2,0) + _R(0,2)),s);
q[0] = dMUL((_R(2,1) - _R(1,2)),s);
return;
case_1:
s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + REAL(1.0));
q[2] = dMUL(REAL(0.5),s);
s = dMUL(REAL(0.5),dRecip(s));
q[3] = dMUL((_R(1,2) + _R(2,1)),s);
q[1] = dMUL((_R(0,1) + _R(1,0)),s);
q[0] = dMUL((_R(0,2) - _R(2,0)),s);
return;
case_2:
s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + REAL(1.0));
q[3] = dMUL(REAL(0.5),s);
s = dMUL(REAL(0.5),dRecip(s));
q[1] = dMUL((_R(2,0) + _R(0,2)),s);
q[2] = dMUL((_R(1,2) + _R(2,1)),s);
q[0] = dMUL((_R(1,0) - _R(0,1)),s);
return;
}
}
int dFactorCholesky (dReal *A, int n)
{
int i,j,k,nskip;
dReal sum,*a,*b,*aa,*bb,*cc,*recip;
dAASSERT (n > 0 && A);
nskip = dPAD (n);
recip = (dReal*) ALLOCA (n * sizeof(dReal));
aa = A;
for (i=0; i<n; i++) {
bb = A;
cc = A + i*nskip;
for (j=0; j<i; j++) {
sum = *cc;
a = aa;
b = bb;
for (k=j; k; k--) sum -= (*(a++))*(*(b++));
*cc = sum * recip[j];
bb += nskip;
cc++;
}
sum = *cc;
a = aa;
for (k=i; k; k--, a++) sum -= (*a)*(*a);
if (sum <= REAL(0.0)) return 0;
*cc = dSqrt(sum);
recip[i] = dRecip (*cc);
aa += nskip;
}
return 1;
}
void dMassSub(dMass *a,const dMass *b)
{
int i;
VERIFY (a && b);
dReal denom = dRecip (a->mass-b->mass);
for (i=0; i<3; ++i) a->c[i] = (a->c[i]*a->mass - b->c[i]*b->mass)*denom;
a->mass-=b->mass;
for (i=0; i<12; ++i) a->I[i] -= b->I[i];
}
void dMassAdd (dMass *a, const dMass *b)
{
int i;
dAASSERT (a && b);
dReal denom = dRecip (a->mass + b->mass);
for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom;
a->mass += b->mass;
for (i=0; i<12; i++) a->I[i] += b->I[i];
}
void cullPoints (int n, double p[], int m, int i0, int iret[])
{
// compute the centroid of the polygon in cx,cy
int i,j;
double a,cx,cy,q;
if (n==1) {
cx = p[0];
cy = p[1];
}
else if (n==2) {
cx = 0.5*(p[0] + p[2]);
cy = 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(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
double A[8];
for (i=0; i<n; i++) A[i] = atan2(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 = double(j)*(2*DART_PI/m) + A[i0];
if (a > DART_PI) a -= 2*DART_PI;
double maxdiff=1e9,diff;
for (i=0; i<n; i++) {
if (avail[i]) {
diff = fabs (A[i]-a);
if (diff > DART_PI) diff = 2*DART_PI - diff;
if (diff < maxdiff) {
maxdiff = diff;
*iret = i;
}
}
}
avail[*iret] = 0;
iret++;
}
}
void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
dReal bx, dReal by, dReal bz)
{
dReal l,k;
dAASSERT (R);
l = dSqrt (ax*ax + ay*ay + az*az);
if (l <= REAL(0.0)) {
dDEBUGMSG ("zero length vector");
memset(R, 0, sizeof(dReal)*12);
return;
}
l = dRecip(l);
ax *= l;
ay *= l;
az *= l;
k = ax*bx + ay*by + az*bz;
bx -= k*ax;
by -= k*ay;
bz -= k*az;
l = dSqrt (bx*bx + by*by + bz*bz);
if (l <= REAL(0.0)) {
memset(R, 0, sizeof(dReal)*12);
dDEBUGMSG ("zero length vector");
return;
}
l = dRecip(l);
bx *= l;
by *= l;
bz *= l;
_R(0,0) = ax;
_R(1,0) = ay;
_R(2,0) = az;
_R(0,1) = bx;
_R(1,1) = by;
_R(2,1) = bz;
_R(0,2) = - by*az + ay*bz;
_R(1,2) = - bz*ax + az*bx;
_R(2,2) = - bx*ay + ax*by;
_R(0,3) = REAL(0.0);
_R(1,3) = REAL(0.0);
_R(2,3) = REAL(0.0);
}
void dLCP::transfer_i_to_C (int i)
{
int j;
if (nC > 0) {
// ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
}
else {
d[0] = dRecip (AROW(i)[i]);
}
swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
C[nC] = nC;
nC++;
# ifdef DEBUG_LCP
checkFactorization (A,L,d,nC,C,nskip);
if (i < (n-1)) checkPermutations (i+1,n,nC,nN,p,C);
# endif
}
EXPORT_C void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
dReal bx, dReal by, dReal bz)
{
dReal l,k;
l = dSqrt (dMUL(ax,ax) + dMUL(ay,ay) + dMUL(az,az));
if (l <= REAL(0.0)) {
return;
}
l = dRecip(l);
ax = dMUL(ax,l);
ay = dMUL(ay,l);
az = dMUL(az,l);
k = dMUL(ax,bx) + dMUL(ay,by) + dMUL(az,bz);
bx -= dMUL(k,ax);
by -= dMUL(k,ay);
bz -= dMUL(k,az);
l = dSqrt (dMUL(bx,bx) + dMUL(by,by) + dMUL(bz,bz));
if (l <= REAL(0.0)) {
return;
}
l = dRecip(l);
bx = dMUL(bx,l);
by = dMUL(by,l);
bz = dMUL(bz,l);
_R(0,0) = ax;
_R(1,0) = ay;
_R(2,0) = az;
_R(0,1) = bx;
_R(1,1) = by;
_R(2,1) = bz;
_R(0,2) = - dMUL(by,az) + dMUL(ay,bz);
_R(1,2) = - dMUL(bz,ax) + dMUL(az,bx);
_R(2,2) = - dMUL(bx,ay) + dMUL(ax,by);
_R(0,3) = REAL(0.0);
_R(1,3) = REAL(0.0);
_R(2,3) = REAL(0.0);
}
void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
const dVector3 pb, const dVector3 ub,
double *alpha, double *beta)
{
dVector3 p;
p[0] = pb[0] - pa[0];
p[1] = pb[1] - pa[1];
p[2] = pb[2] - pa[2];
double uaub = Inner(ua,ub);
double q1 = Inner(ua,p);
double q2 = -Inner(ub,p);
double d = 1-uaub*uaub;
if (d <= 0) {
// @@@ this needs to be made more robust
*alpha = 0;
*beta = 0;
}
else {
d = dRecip(d);
*alpha = (q1 + uaub*q2)*d;
*beta = (uaub*q1 + q2)*d;
}
}
int _dFactorCholesky (dReal *A, int n, void *tmpbuf/*[n]*/)
{
dAASSERT (n > 0 && A);
bool failure = false;
const int nskip = dPAD (n);
dReal *recip = tmpbuf ? (dReal *)tmpbuf : (dReal*) ALLOCA (n * sizeof(dReal));
dReal *aa = A;
for (int i=0; i<n; aa+=nskip, ++i) {
dReal *cc = aa;
{
const dReal *bb = A;
for (int j=0; j<i; bb+=nskip, ++cc, ++j) {
dReal sum = *cc;
const dReal *a = aa, *b = bb, *bend = bb + j;
for (; b != bend; ++a, ++b) {
sum -= (*a)*(*b);
}
*cc = sum * recip[j];
}
}
{
dReal sum = *cc;
dReal *a = aa, *aend = aa + i;
for (; a != aend; ++a) {
sum -= (*a)*(*a);
}
if (sum <= REAL(0.0)) {
failure = true;
break;
}
dReal sumsqrt = dSqrt(sum);
*cc = sumsqrt;
recip[i] = dRecip (sumsqrt);
}
}
return failure ? 0 : 1;
}
void
dxJointUniversal::getAngles( dReal *angle1, dReal *angle2 )
{
if ( node[0].body )
{
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
dMatrix3 R;
dQuaternion qcross, qq, qrel;
getAxes( ax1, ax2 );
// It should be possible to get both angles without explicitly
// constructing the rotation matrix of the cross. Basically,
// orientation of the cross about axis1 comes from body 2,
// about axis 2 comes from body 1, and the perpendicular
// axis can come from the two bodies somehow. (We don't really
// want to assume it's 90 degrees, because in general the
// constraints won't be perfectly satisfied, or even very well
// satisfied.)
//
// However, we'd need a version of getHingeAngleFromRElativeQuat()
// that CAN handle when its relative quat is rotated along a direction
// other than the given axis. What I have here works,
// although it's probably much slower than need be.
dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] );
dRtoQ( R, qcross );
// This code is essentialy the same as getHingeAngle(), see the comments
// there for details.
// get qrel = relative rotation between node[0] and the cross
dQMultiply1( qq, node[0].body->q, qcross );
dQMultiply2( qrel, qq, qrel1 );
*angle1 = getHingeAngleFromRelativeQuat( qrel, axis1 );
// This is equivalent to
// dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
// You see that the R is constructed from the same 2 axis as for angle1
// but the first and second axis are swapped.
// So we can take the first R and rapply a rotation to it.
// The rotation is around the axis between the 2 axes (ax1 and ax2).
// We do a rotation of 180deg.
dQuaternion qcross2;
// Find the vector between ax1 and ax2 (i.e. in the middle)
// We need to turn around this vector by 180deg
// The 2 axes should be normalize so to find the vector between the 2.
// Add and devide by 2 then normalize or simply normalize
// ax2
// ^
// |
// |
/// *------------> ax1
// We want the vector a 45deg
//
// N.B. We don't need to normalize the ax1 and ax2 since there are
// normalized when we set them.
// We set the quaternion q = [cos(theta), dir*sin(theta)] = [w, x, y, Z]
qrel[0] = 0; // equivalent to cos(Pi/2)
qrel[1] = ax1[0] + ax2[0]; // equivalent to x*sin(Pi/2); since sin(Pi/2) = 1
qrel[2] = ax1[1] + ax2[1];
qrel[3] = ax1[2] + ax2[2];
dReal l = dRecip( sqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] ) );
qrel[1] *= l;
qrel[2] *= l;
qrel[3] *= l;
dQMultiply0( qcross2, qrel, qcross );
if ( node[1].body )
{
dQMultiply1( qq, node[1].body->q, qcross2 );
dQMultiply2( qrel, qq, qrel2 );
}
else
{
// pretend joint->node[1].body->q is the identity
dQMultiply2( qrel, qcross2, qrel2 );
}
*angle2 = - getHingeAngleFromRelativeQuat( qrel, axis2 );
}
else
{
*angle1 = 0;
*angle2 = 0;
}
}
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 = dCalcVectorDot3_44(R1+0,R2+0); R12 = dCalcVectorDot3_44(R1+0,R2+1); R13 = dCalcVectorDot3_44(R1+0,R2+2);
R21 = dCalcVectorDot3_44(R1+1,R2+0); R22 = dCalcVectorDot3_44(R1+1,R2+1); R23 = dCalcVectorDot3_44(R1+1,R2+2);
R31 = dCalcVectorDot3_44(R1+2,R2+0); R32 = dCalcVectorDot3_44(R1+2,R2+1); R33 = dCalcVectorDot3_44(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 (dCalcVectorDot3_41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
TST (dCalcVectorDot3_41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
TST (dCalcVectorDot3_41(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 = (dCalcVectorDot3_14(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 = (dCalcVectorDot3_14(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 = dCalcVectorDot3_14 (center,Ra+code1);
c2 = dCalcVectorDot3_14 (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 = dCalcVectorDot3_44 (Ra+code1,Rb+a1);
m12 = dCalcVectorDot3_44 (Ra+code1,Rb+a2);
m21 = dCalcVectorDot3_44 (Ra+code2,Rb+a1);
m22 = dCalcVectorDot3_44 (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] - dCalcVectorDot3(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 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++;
}
}
// 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 _dFactorLDLT (dReal *A, dReal *d, int n, int nskip1)
{
int i,j;
dReal sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22;
if (n < 1) return;
for (i=0; i<=n-2; i += 2) {
/* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
dSolveL1_2 (A,A+i*nskip1,i,nskip1);
/* scale the elements in a 2 x i block at A(i,0), and also */
/* compute Z = the outer product matrix that we'll need. */
Z11 = 0;
Z21 = 0;
Z22 = 0;
ell = A+i*nskip1;
dee = d;
#pragma kaapi loop \
reduction(reduce_sum:Z11, reduce_sum:Z21, reduce_sum:Z22)
for (j=i-6; j >= 0; j -= 6, ell += 6, dee += 6) {
dReal _Z11 = 0;
dReal _Z21 = 0;
dReal _Z22 = 0;
p1 = ell[0];
p2 = ell[nskip1];
dd = dee[0];
q1 = p1*dd;
q2 = p2*dd;
ell[0] = q1;
ell[nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[1];
p2 = ell[1+nskip1];
dd = dee[1];
q1 = p1*dd;
q2 = p2*dd;
ell[1] = q1;
ell[1+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[2];
p2 = ell[2+nskip1];
dd = dee[2];
q1 = p1*dd;
q2 = p2*dd;
ell[2] = q1;
ell[2+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[3];
p2 = ell[3+nskip1];
dd = dee[3];
q1 = p1*dd;
q2 = p2*dd;
ell[3] = q1;
ell[3+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[4];
p2 = ell[4+nskip1];
dd = dee[4];
q1 = p1*dd;
q2 = p2*dd;
ell[4] = q1;
ell[4+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[5];
p2 = ell[5+nskip1];
dd = dee[5];
q1 = p1*dd;
q2 = p2*dd;
ell[5] = q1;
ell[5+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
}
/* xkaapi does not yet update affine variables */
fixme_skip:
if ((i - 6) >= 0)
{
static const int step = 6;
const int range_size = (i - 6 + 1) - 0;
int niter = range_size / step;
if (range_size % step) niter += 1;
j = (i - 6) - niter * 6;
ell = (A + i * nskip1) + niter * 6;
dee = d + niter * 6;
}
else
{
j = i - 6;
}
/* compute left-over iterations */
j += 6;
for (; j > 0; j--) {
p1 = ell[0];
p2 = ell[nskip1];
dd = dee[0];
q1 = p1*dd;
q2 = p2*dd;
ell[0] = q1;
ell[nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
ell++;
dee++;
}
/* solve for diagonal 2 x 2 block at A(i,i) */
Z11 = ell[0] - Z11;
Z21 = ell[nskip1] - Z21;
Z22 = ell[1+nskip1] - Z22;
dee = d + i;
/* factorize 2 x 2 block Z,dee */
/* factorize row 1 */
dee[0] = dRecip(Z11);
/* factorize row 2 */
sum = 0;
q1 = Z21;
q2 = q1 * dee[0];
Z21 = q2;
sum += q1*q2;
dee[1] = dRecip(Z22 - sum);
/* done factorizing 2 x 2 block */
ell[nskip1] = Z21;
}
/* compute the (less than 2) rows at the bottom */
switch (n-i) {
case 0:
break;
case 1:
dSolveL1_1 (A,A+i*nskip1,i,nskip1);
/* scale the elements in a 1 x i block at A(i,0), and also */
/* compute Z = the outer product matrix that we'll need. */
Z11 = 0;
ell = A+i*nskip1;
dee = d;
for (j=i-6; j >= 0; j -= 6) {
p1 = ell[0];
dd = dee[0];
q1 = p1*dd;
ell[0] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[1];
dd = dee[1];
q1 = p1*dd;
ell[1] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[2];
dd = dee[2];
q1 = p1*dd;
ell[2] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[3];
dd = dee[3];
q1 = p1*dd;
ell[3] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[4];
dd = dee[4];
q1 = p1*dd;
ell[4] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[5];
dd = dee[5];
q1 = p1*dd;
ell[5] = q1;
m11 = p1*q1;
Z11 += m11;
ell += 6;
dee += 6;
}
/* compute left-over iterations */
j += 6;
for (; j > 0; j--) {
p1 = ell[0];
dd = dee[0];
q1 = p1*dd;
ell[0] = q1;
m11 = p1*q1;
Z11 += m11;
ell++;
dee++;
}
/* solve for diagonal 1 x 1 block at A(i,i) */
Z11 = ell[0] - Z11;
dee = d + i;
/* factorize 1 x 1 block Z,dee */
/* factorize row 1 */
dee[0] = dRecip(Z11);
/* done factorizing 1 x 1 block */
break;
default: *((char*)0)=0; /* this should never happen! */
}
}
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (SphereGeom->type == dSphereClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == SphereGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
SphereCollider& Collider = pccColliderCache->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
dCopyVector3(Sphere.mCenter, Position);
Sphere.mRadius = Radius;
Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
if (!sphereTC){
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
sphereTC->Geom = SphereGeom;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
if (! Collider.GetContactStatus()) {
// no collision occurred
return 0;
}
// get results
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0){
if (TriMesh->ArrayCallback != null){
TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
}
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++){
if (OutTriCount == (Flags & NUMC_MASK)){
break;
}
const int TriIndex = Triangles[i];
dVector3 dv[3];
if (!Callback(TriMesh, SphereGeom, TriIndex))
continue;
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
dSubtractVectors3r4(vu, v1, v0);
vu[3] = REAL(0.0);
dVector3 vv;
dSubtractVectors3r4(vv, v2, v0);
vv[3] = REAL(0.0);
// Get plane coefficients
dVector4 Plane;
dCalcVectorCross3(Plane, vu, vv);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(Plane)) {
continue;
}
/* 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).
*/
dReal side = dCalcVectorDot3(Plane, Position) - dCalcVectorDot3(Plane, v0);
if(side < REAL(0.0))
{
continue;
}
dReal Depth;
dReal u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesn't hit triangle
}
if (Depth < REAL(0.0)){
continue; // Negative depth does not produce a contact
}
dVector3 ContactPos;
dReal w = REAL(1.0) - u - v;
dAddScaledVectors3r4(ContactPos, v1, v2, u, v);
dAddScaledVector3r4(ContactPos, v0, w);
// Depth returned from GetContactData is depth along
// contact point - sphere center direction
// we'll project it to contact normal
dVector3 dir;
dSubtractVectors3r4(dir, Position, ContactPos);
dReal dirProj = dCalcVectorDot3(dir, Plane) / dCalcVectorLength3(dir);
// Since Depth already had a requirement to be non-negative,
// negative direction projections should not be allowed as well,
// as otherwise the multiplication will result in negative contact depth.
if (dirProj < REAL(0.0))
continue; // Zero contact depth could be ignored
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dCopyVector3r4(Contact->pos, ContactPos);
// Using normal as plane (reversed)
dCopyNegatedVector3r4(Contact->normal, Plane);
Contact->depth = Depth * dirProj;
//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
// We need to set these unconditionally, as the merging may fail! - Bram
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
Contact->side1 = TriIndex;
OutTriCount++;
}
if (OutTriCount > 0)
{
if (TriMesh->SphereContactsMergeOption == MERGE_CONTACTS_FULLY)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT))
{
dVector3 pos;
dCopyVector3r4(pos, Contact->pos);
dVector3 normal;
dCopyScaledVector3r4(normal, Contact->normal, Contact->depth);
int TriIndex = Contact->side1;
for (int i = 1; i < OutTriCount; i++)
{
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
dAddVector3r4(pos, TempContact->pos);
dAddScaledVector3r4(normal, TempContact->normal, TempContact->depth);
TriIndex = (TriMesh->TriMergeCallback) ? TriMesh->TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1;
}
Contact->side1 = TriIndex;
dReal invOutTriCount = dRecip(OutTriCount);
dCopyScaledVector3r4(Contact->pos, pos, invOutTriCount);
if ( !dSafeNormalize3(normal) )
return OutTriCount; // Cannot merge in this pathological case
// Using a merged normal, means that for each intersection, this new normal will be less effective in solving the intersection.
// That is why we need to correct this by increasing the depth for each intersection.
// The maximum of the adjusted depths is our newly merged depth value - Bram.
dReal mergedDepth = REAL(0.0);
dReal minEffectiveness = REAL(0.5);
for ( int i = 0; i < OutTriCount; ++i )
{
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
dReal effectiveness = dCalcVectorDot3(normal, TempContact->normal);
if ( effectiveness < dEpsilon )
return OutTriCount; // Cannot merge this pathological case
// Cap our adjustment for the new normal to a factor 2, meaning a 60 deg change in normal.
effectiveness = ( effectiveness < minEffectiveness ) ? minEffectiveness : effectiveness;
dReal adjusted = TempContact->depth / effectiveness;
mergedDepth = ( mergedDepth < adjusted ) ? adjusted : mergedDepth;
}
Contact->depth = mergedDepth;
dCopyVector3r4(Contact->normal, normal);
}
return 1;
}
else if (TriMesh->SphereContactsMergeOption == MERGE_CONTACT_NORMALS)
{
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT))
{
dVector3 Normal;
dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride);
dCopyScaledVector3r4(Normal, FirstContact->normal, FirstContact->depth);
for (int i = 1; i < OutTriCount; i++)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
dAddScaledVector3r4(Normal, Contact->normal, Contact->depth);
}
dNormalize3(Normal);
for (int i = 0; i < OutTriCount; i++)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
dCopyVector3r4(Contact->normal, Normal);
}
}
return OutTriCount;
}
else
{
dIASSERT(TriMesh->SphereContactsMergeOption == DONT_MERGE_CONTACTS);
return OutTriCount;
}
}
else return 0;
}
else return 0;
}
int dCollideRayCapsule (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dRayClass);
dIASSERT (o2->type == dCapsuleClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxRay *ray = (dxRay*) o1;
dxCapsule *ccyl = (dxCapsule*) o2;
contact->g1 = ray;
contact->g2 = ccyl;
contact->side1 = -1;
contact->side2 = -1;
dReal lz2 = ccyl->lz * REAL(0.5);
// compute some useful info
dVector3 cs,q,r;
dReal C,k;
cs[0] = ray->final_posr->pos[0] - ccyl->final_posr->pos[0];
cs[1] = ray->final_posr->pos[1] - ccyl->final_posr->pos[1];
cs[2] = ray->final_posr->pos[2] - ccyl->final_posr->pos[2];
k = dCalcVectorDot3_41(ccyl->final_posr->R+2,cs); // position of ray start along ccyl axis
q[0] = k*ccyl->final_posr->R[0*4+2] - cs[0];
q[1] = k*ccyl->final_posr->R[1*4+2] - cs[1];
q[2] = k*ccyl->final_posr->R[2*4+2] - cs[2];
C = dCalcVectorDot3(q,q) - ccyl->radius*ccyl->radius;
// if C < 0 then ray start position within infinite extension of cylinder
// see if ray start position is inside the capped cylinder
int inside_ccyl = 0;
if (C < 0) {
if (k < -lz2) k = -lz2;
else if (k > lz2) k = lz2;
r[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
r[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
r[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
if ((ray->final_posr->pos[0]-r[0])*(ray->final_posr->pos[0]-r[0]) +
(ray->final_posr->pos[1]-r[1])*(ray->final_posr->pos[1]-r[1]) +
(ray->final_posr->pos[2]-r[2])*(ray->final_posr->pos[2]-r[2]) < ccyl->radius*ccyl->radius) {
inside_ccyl = 1;
}
}
// compute ray collision with infinite cylinder, except for the case where
// the ray is outside the capped cylinder but within the infinite cylinder
// (it that case the ray can only hit endcaps)
if (!inside_ccyl && C < 0) {
// set k to cap position to check
if (k < 0) k = -lz2; else k = lz2;
}
else {
dReal uv = dCalcVectorDot3_44(ccyl->final_posr->R+2,ray->final_posr->R+2);
r[0] = uv*ccyl->final_posr->R[0*4+2] - ray->final_posr->R[0*4+2];
r[1] = uv*ccyl->final_posr->R[1*4+2] - ray->final_posr->R[1*4+2];
r[2] = uv*ccyl->final_posr->R[2*4+2] - ray->final_posr->R[2*4+2];
dReal A = dCalcVectorDot3(r,r);
dReal B = 2*dCalcVectorDot3(q,r);
k = B*B-4*A*C;
if (k < 0) {
// the ray does not intersect the infinite cylinder, but if the ray is
// inside and parallel to the cylinder axis it may intersect the end
// caps. set k to cap position to check.
if (!inside_ccyl) return 0;
if (uv < 0) k = -lz2; else k = lz2;
}
else {
k = dSqrt(k);
A = dRecip (2*A);
dReal alpha = (-B-k)*A;
if (alpha < 0) {
alpha = (-B+k)*A;
if (alpha < 0) return 0;
}
if (alpha > ray->length) return 0;
// the ray intersects the infinite cylinder. check to see if the
// intersection point is between the caps
contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
q[0] = contact->pos[0] - ccyl->final_posr->pos[0];
q[1] = contact->pos[1] - ccyl->final_posr->pos[1];
q[2] = contact->pos[2] - ccyl->final_posr->pos[2];
k = dCalcVectorDot3_14(q,ccyl->final_posr->R+2);
dReal nsign = inside_ccyl ? REAL(-1.0) : REAL(1.0);
if (k >= -lz2 && k <= lz2) {
contact->normal[0] = nsign * (contact->pos[0] -
(ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2]));
contact->normal[1] = nsign * (contact->pos[1] -
(ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2]));
contact->normal[2] = nsign * (contact->pos[2] -
(ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2]));
dNormalize3 (contact->normal);
contact->depth = alpha;
return 1;
}
// the infinite cylinder intersection point is not between the caps.
// set k to cap position to check.
if (k < 0) k = -lz2; else k = lz2;
}
}
// check for ray intersection with the caps. k must indicate the cap
// position to check
q[0] = ccyl->final_posr->pos[0] + k*ccyl->final_posr->R[0*4+2];
q[1] = ccyl->final_posr->pos[1] + k*ccyl->final_posr->R[1*4+2];
q[2] = ccyl->final_posr->pos[2] + k*ccyl->final_posr->R[2*4+2];
return ray_sphere_helper (ray,q,ccyl->radius,contact, inside_ccyl);
}
fc_ptr[3] += delta * iMJ_ptr[9];
fc_ptr[4] += delta * iMJ_ptr[10];
fc_ptr[5] += delta * iMJ_ptr[11];
}
}
}
}
void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb,
dxJoint * const *_joint, int nj, dReal stepsize)
{
int i,j;
IFTIMING(dTimerStart("preprocessing");)
dReal stepsize1 = dRecip(stepsize);
// number all bodies in the body list - set their tag values
for (i=0; i<nb; i++) body[i]->tag = i;
// make a local copy of the joint array, because we might want to modify it.
// (the "dxJoint *const*" declaration says we're allowed to modify the joints
// but not the joint array, because the caller might need it unchanged).
//@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints
dxJoint **joint = (dxJoint**) alloca (nj * sizeof(dxJoint*));
memcpy (joint,_joint,nj * sizeof(dxJoint*));
// for all bodies, compute the inertia tensor and its inverse in the global
// frame, and compute the rotational force and add it to the torque
// accumulator. I and invI are a vertical stack of 3x4 matrices, one per body.
dRealAllocaArray (I,3*4*nb); // need to remember all I's for feedback purposes only
// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
// generate contact points. this returns 0 if there is no contact otherwise
// it returns the number of contacts generated.
// `normal' returns the contact normal.
// `depth' returns the maximum penetration depth along that normal.
// `return_code' returns a number indicating the type of contact that was
// detected:
// 1,2,3 = box 2 intersects with a face of box 1
// 4,5,6 = box 1 intersects with a face of box 2
// 7..15 = edge-edge contact
// `maxc' is the maximum number of contacts allowed to be generated, i.e.
// the size of the `contact' array.
// `contact' and `skip' are the contact array information provided to the
// collision functions. this function only fills in the position and depth
// fields.
int dBoxBox(const dVector3 p1, const dMatrix3 R1, const dVector3 side1,
const dVector3 p2, const dMatrix3 R2, const dVector3 side2,
std::vector<Contact>& result)
{
const double fudge_factor = 1.05;
dVector3 p,pp,normalC = {0.0, 0.0, 0.0, 0.0};
const double *normalR = 0;
double 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;
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];
A[1] = side1[1];
A[2] = side1[2];
B[0] = side2[0];
B[1] = side2[1];
B[2] = side2[2];
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = Inner44(R1+0,R2+0); R12 = Inner44(R1+0,R2+1); R13 = Inner44(R1+0,R2+2);
R21 = Inner44(R1+1,R2+0); R22 = Inner44(R1+1,R2+1); R23 = Inner44(R1+1,R2+2);
R31 = Inner44(R1+2,R2+0); R32 = Inner44(R1+2,R2+1); R33 = Inner44(R1+2,R2+2);
Q11 = std::abs(R11); Q12 = std::abs(R12); Q13 = std::abs(R13);
Q21 = std::abs(R21); Q22 = std::abs(R22); Q23 = std::abs(R23);
Q31 = std::abs(R31); Q32 = std::abs(R32); Q33 = std::abs(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.
#define TST(expr1,expr2,norm,cc) \
s2 = std::abs(expr1) - (expr2); \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1) < 0); \
code = (cc); \
}
s = -1E12;
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 (Inner41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
TST (Inner41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
TST (Inner41(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) \
s2 = std::abs(expr1) - (expr2); \
l = sqrt ((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) < 0); \
code = (cc); \
} \
}
// 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
if (!code) return 0;
if (s > 0.0) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
Eigen::Vector3d normal;
Eigen::Vector3d point_vec;
double penetration;
if (normalR) {
normal << normalR[0],normalR[4],normalR[8];
}
else {
normal << Inner((R1),(normalC)), Inner((R1+4),(normalC)), Inner((R1+8),(normalC));
//dMULTIPLY0_331 (normal,R1,normalC);
}
if (invert_normal) {
normal *= -1.0;
}
// compute contact point(s)
// single point
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;
double sign;
for (i=0; i<3; i++) pa[i] = p1[i];
for (j=0; j<3; j++)
{
#define TEMP_INNER14(a,b) (a[0]*(b)[0] + a[1]*(b)[4] + a[2]*(b)[8])
sign = (TEMP_INNER14(normal,R1+j) > 0) ? 1.0 : -1.0;
//sign = (Inner14(normal,R1+j) > 0) ? 1.0 : -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;
for (i=0; i<3; i++) pb[i] = p2[i];
for (j=0; j<3; j++) {
sign = (TEMP_INNER14(normal,R2+j) > 0) ? -1.0 : 1.0;
#undef TEMP_INNER14
for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
}
double alpha,beta;
dVector3 ua,ub;
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
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;
{
point_vec << 0.5*(pa[0]+pb[0]), 0.5*(pa[1]+pb[1]), 0.5*(pa[2]+pb[2]);
penetration = -s;
Contact contact;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.push_back(contact);
}
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).
const double *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) {
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else {
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
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];
}
dMULTIPLY1_331 (nr,Rb,normal2);
anr[0] = fabs (nr[0]);
anr[1] = fabs (nr[1]);
anr[2] = fabs (nr[2]);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent 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
double quad[8]; // 2D coordinate of incident face (x,y pairs)
double c1,c2,m11,m12,m21,m22;
c1 = Inner14 (center,Ra+code1);
c2 = Inner14 (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 = Inner44 (Ra+code1,Rb+a1);
m12 = Inner44 (Ra+code1,Rb+a2);
m21 = Inner44 (Ra+code2,Rb+a1);
m22 = Inner44 (Ra+code2,Rb+a2);
{
double k1 = m11*Sb[a1];
double k2 = m21*Sb[a1];
double k3 = m12*Sb[a2];
double 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
double rect[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
double 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.
//real point[3*8]; // penetrating contact points
double point[24]; // penetrating contact points
double dep[8]; // depths for those points
double 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++) {
double k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
double 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] - Inner(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 < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
int maxc = 4;
if (maxc > cnum) maxc = cnum;
//if (maxc < 1) maxc = 1;
if (cnum <= maxc) {
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++)
{
point_vec << point[j*3+0] + pa[0], point[j*3+1] + pa[1], point[j*3+2] + pa[2];
Contact contact;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = dep[j];
result.push_back(contact);
}
}
else {
// 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;
double 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);
cnum = maxc;
for (j=0; j < cnum; j++)
{
point_vec << point[iret[j]*3+0] + pa[0], point[iret[j]*3+1] + pa[1], point[iret[j]*3+2] + pa[2];
Contact contact;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = dep[iret[j]];
result.push_back(contact);
}
}
return cnum;
}
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 = dMUL(REAL(0.5),(p[0] + p[2]));
cy = dMUL(REAL(0.5),(p[1] + p[3]));
}
else {
a = 0;
cx = 0;
cy = 0;
for (i=0; i<(n-1); i++) {
q = dMUL(p[i*2],p[i*2+3]) - dMUL(p[i*2+2],p[i*2+1]);
a += q;
cx += dMUL(q,(p[i*2]+p[i*2+2]));
cy += dMUL(q,(p[i*2+1]+p[i*2+3]));
}
q = dMUL(p[n*2-2],p[1]) - dMUL(p[0],p[n*2-1]);
a = dRecip(dMUL(REAL(3.0),(a+q)));
cx = dMUL(a,(cx + dMUL(q,(p[n*2-2]+p[0]))));
cy = dMUL(a,(cy + dMUL(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] = dArcTan2(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 = dMUL(REAL(j),(2*dPI/m)) + A[i0];
if (a > dPI) a -= 2*dPI;
dReal maxdiff = REAL(1e9);
dReal 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 > dPI) diff = 2*dPI - diff;
if (diff < maxdiff) {
maxdiff = diff;
*iret = i;
}
}
}
avail[*iret] = 0;
iret++;
}
}
// Ray - Cylinder collider by David Walters (June 2006)
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 );
//
// Compute some useful info
//
dVector3 q, r;
dReal d, C, k;
// Vector 'r', line segment from C to R (ray start) ( r = R - C )
r[ 0 ] = ray->final_posr->pos[0] - cyl->final_posr->pos[0];
r[ 1 ] = ray->final_posr->pos[1] - cyl->final_posr->pos[1];
r[ 2 ] = ray->final_posr->pos[2] - cyl->final_posr->pos[2];
// Distance that ray start is along cyl axis ( Z-axis direction )
d = dCalcVectorDot3_41( cyl->final_posr->R + 2, r );
//
// Compute vector 'q' representing the shortest line from R to the cylinder z-axis (Cz).
//
// Point on axis ( in world space ): cp = ( d * Cz ) + C
//
// Line 'q' from R to cp: q = cp - R
// q = ( d * Cz ) + C - R
// q = ( d * Cz ) - ( R - C )
q[ 0 ] = ( d * cyl->final_posr->R[0*4+2] ) - r[ 0 ];
q[ 1 ] = ( d * cyl->final_posr->R[1*4+2] ) - r[ 1 ];
q[ 2 ] = ( d * cyl->final_posr->R[2*4+2] ) - r[ 2 ];
// Compute square length of 'q'. Subtract from radius squared to
// get square distance 'C' between the line q and the radius.
// if C < 0 then ray start position is within infinite extension of cylinder
C = dCalcVectorDot3( q, q ) - ( cyl->radius * cyl->radius );
// Compute the projection of ray direction normal onto cylinder direction normal.
dReal uv = dCalcVectorDot3_44( cyl->final_posr->R+2, ray->final_posr->R+2 );
//
// Find ray collision with infinite cylinder
//
// Compute vector from end of ray direction normal to projection on cylinder direction normal.
r[ 0 ] = ( uv * cyl->final_posr->R[0*4+2] ) - ray->final_posr->R[0*4+2];
r[ 1 ] = ( uv * cyl->final_posr->R[1*4+2] ) - ray->final_posr->R[1*4+2];
r[ 2 ] = ( uv * cyl->final_posr->R[2*4+2] ) - ray->final_posr->R[2*4+2];
// Quadratic Formula Magic
// Compute discriminant 'k':
// k < 0 : No intersection
// k = 0 : Tangent
// k > 0 : Intersection
dReal A = dCalcVectorDot3( r, r );
dReal B = 2 * dCalcVectorDot3( q, r );
k = B*B - 4*A*C;
//
// Collision with Flat Caps ?
//
// No collision with cylinder edge. ( Use epsilon here or we miss some obvious cases )
if ( k < dEpsilon && C <= 0 )
{
// The ray does not intersect the edge of the infinite cylinder,
// but the ray start is inside and so must run parallel to the axis.
// It may yet intersect an end cap. The following cases are valid:
// -ve-cap , -half centre +half , +ve-cap
// <<================|-------------------|------------->>>---|================>>
// | |
// | d-------------------> 1.
// 2. d------------------> |
// 3. <------------------d |
// | <-------------------d 4.
// | |
// <<================|-------------------|------------->>>---|===============>>
// Negative if the ray and cylinder axes point in opposite directions.
const dReal uvsign = ( uv < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
// Negative if the ray start is inside the cylinder
const dReal internal = ( d >= -half_length && d <= +half_length ) ? REAL( -1.0 ) : REAL( 1.0 );
// Ray and Cylinder axes run in the same direction ( cases 1, 2 )
// Ray and Cylinder axes run in opposite directions ( cases 3, 4 )
if ( ( ( uv > 0 ) && ( d + ( uvsign * ray->length ) < half_length * internal ) ) ||
( ( uv < 0 ) && ( d + ( uvsign * ray->length ) > half_length * internal ) ) )
{
return 0; // No intersection with caps or curved surface.
}
// Compute depth (distance from ray to cylinder)
contact->depth = ( ( -uvsign * d ) - ( internal * half_length ) );
// Compute contact point.
contact->pos[0] = ray->final_posr->pos[0] + ( contact->depth * ray->final_posr->R[0*4+2] );
contact->pos[1] = ray->final_posr->pos[1] + ( contact->depth * ray->final_posr->R[1*4+2] );
contact->pos[2] = ray->final_posr->pos[2] + ( contact->depth * ray->final_posr->R[2*4+2] );
// Compute reflected contact normal.
contact->normal[0] = uvsign * ( cyl->final_posr->R[0*4+2] );
contact->normal[1] = uvsign * ( cyl->final_posr->R[1*4+2] );
contact->normal[2] = uvsign * ( cyl->final_posr->R[2*4+2] );
// Contact!
return 1;
}
//
// Collision with Curved Edge ?
//
if ( k > 0 )
{
// Finish off quadratic formula to get intersection co-efficient
k = dSqrt( k );
A = dRecip( 2 * A );
// Compute distance along line to contact point.
dReal alpha = ( -B - k ) * A;
if ( alpha < 0 )
{
// Flip in the other direction.
alpha = ( -B + k ) * A;
}
// Intersection point is within ray length?
if ( alpha >= 0 && alpha <= ray->length )
{
// The ray intersects the infinite cylinder!
// Compute contact point.
contact->pos[0] = ray->final_posr->pos[0] + ( alpha * ray->final_posr->R[0*4+2] );
contact->pos[1] = ray->final_posr->pos[1] + ( alpha * ray->final_posr->R[1*4+2] );
contact->pos[2] = ray->final_posr->pos[2] + ( alpha * ray->final_posr->R[2*4+2] );
// q is the vector from the cylinder centre to the contact point.
q[0] = contact->pos[0] - cyl->final_posr->pos[0];
q[1] = contact->pos[1] - cyl->final_posr->pos[1];
q[2] = contact->pos[2] - cyl->final_posr->pos[2];
// Compute the distance along the cylinder axis of this contact point.
d = dCalcVectorDot3_14( q, cyl->final_posr->R+2 );
// Check to see if the intersection point is between the flat end caps
if ( d >= -half_length && d <= +half_length )
{
// Flip the normal if the start point is inside the cylinder.
const dReal nsign = ( C < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
// Compute contact normal.
contact->normal[0] = nsign * (contact->pos[0] - (cyl->final_posr->pos[0] + d*cyl->final_posr->R[0*4+2]));
contact->normal[1] = nsign * (contact->pos[1] - (cyl->final_posr->pos[1] + d*cyl->final_posr->R[1*4+2]));
contact->normal[2] = nsign * (contact->pos[2] - (cyl->final_posr->pos[2] + d*cyl->final_posr->R[2*4+2]));
dNormalize3( contact->normal );
// Store depth.
contact->depth = alpha;
// Contact!
return 1;
}
}
}
// No contact with anything.
return 0;
}
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Cooper(btScalar step) const
{
#if 0
dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size
dVector3 L; // Compute angular momentum
dMultiply0_331(L, I, b->avel);
#endif
btVector3 inertiaLocal = getLocalInertia();
btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
btVector3 L = inertiaTensorWorld*getAngularVelocity();
btMatrix3x3 Itild(0, 0, 0, 0, 0, 0, 0, 0, 0);
#if 0
for (int ii = 0; ii<12; ++ii) {
Itild[ii] = Itild[ii] * h + I[ii];
}
#endif
btSetCrossMatrixMinus(Itild, L*step);
Itild += inertiaTensorWorld;
#if 0
// Compute a new effective 'inertia tensor'
// for the implicit step: the cross-product
// matrix of the angular momentum plus the
// old tensor scaled by the timestep.
// Itild may not be symmetric pos-definite,
// but we can still use it to compute implicit
// gyroscopic torques.
dMatrix3 Itild = { 0 };
dSetCrossMatrixMinus(Itild, L, 4);
for (int ii = 0; ii<12; ++ii) {
Itild[ii] = Itild[ii] * h + I[ii];
}
#endif
L *= step;
//Itild may not be symmetric pos-definite
btMatrix3x3 itInv = Itild.inverse();
Itild = inertiaTensorWorld * itInv;
btMatrix3x3 ident(1,0,0,0,1,0,0,0,1);
Itild -= ident;
#if 0
// Scale momentum by inverse time to get
// a sort of "torque"
dScaleVector3(L, dRecip(h));
// Invert the pseudo-tensor
dMatrix3 itInv;
// This is a closed-form inversion.
// It's probably not numerically stable
// when dealing with small masses with
// a large asymmetry.
// An LU decomposition might be better.
if (dInvertMatrix3(itInv, Itild) != 0) {
// "Divide" the original tensor
// by the pseudo-tensor (on the right)
dMultiply0_333(Itild, I, itInv);
// Subtract an identity matrix
Itild[0] -= 1; Itild[5] -= 1; Itild[10] -= 1;
// This new inertia matrix rotates the
// momentum to get a new set of torques
// that will work correctly when applied
// to the old inertia matrix as explicit
// torques with a semi-implicit update
// step.
dVector3 tau0;
dMultiply0_331(tau0, Itild, L);
// Add the gyro torques to the torque
// accumulator
for (int ii = 0; ii<3; ++ii) {
b->tacc[ii] += tau0[ii];
}
#endif
btVector3 tau0 = Itild * L;
// printf("tau0 = %f,%f,%f\n",tau0.x(),tau0.y(),tau0.z());
return tau0;
}
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(btScalar step) const
{
// use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior.
// calculate using implicit euler step so it's stable.
const btVector3 inertiaLocal = getLocalInertia();
const btVector3 w0 = getAngularVelocity();
btMatrix3x3 I;
I = m_worldTransform.getBasis().scaled(inertiaLocal) *
m_worldTransform.getBasis().transpose();
// use newtons method to find implicit solution for new angular velocity (w')
// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
// df/dw' = I + 1xIw'*step + w'xI*step
btVector3 w1 = w0;
// one step of newton's method
{
const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
const btMatrix3x3 dfw_inv = dfw.inverse();
btVector3 dw;
dw = dfw_inv*fw;
w1 -= dw;
}
btVector3 gf = (w1 - w0);
return gf;
}
void btRigidBody::integrateVelocities(btScalar step)
{
if (isStaticOrKinematicObject())
return;
m_linearVelocity += m_totalForce * (m_inverseMass * step);
m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
#define MAX_ANGVEL SIMD_HALF_PI
/// clamp angular velocity. collision calculations will fail on higher angular velocities
btScalar angvel = m_angularVelocity.length();
if (angvel*step > MAX_ANGVEL)
{
m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
}
}
btQuaternion btRigidBody::getOrientation() const
{
btQuaternion orn;
m_worldTransform.getBasis().getRotation(orn);
return orn;
}
void dFactorLDLT (dReal *A, dReal *d, int n, int nskip1)
{
int i,j;
dReal sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22;
if (n < 1) return;
for (i=0; i<=n-2; i += 2) {
/* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
dSolveL1_2 (A,A+i*nskip1,i,nskip1);
/* scale the elements in a 2 x i block at A(i,0), and also */
/* compute Z = the outer product matrix that we'll need. */
Z11 = 0;
Z21 = 0;
Z22 = 0;
ell = A+i*nskip1;
dee = d;
for (j=i-6; j >= 0; j -= 6) {
p1 = ell[0];
p2 = ell[nskip1];
dd = dee[0];
q1 = p1*dd;
q2 = p2*dd;
ell[0] = q1;
ell[nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[1];
p2 = ell[1+nskip1];
dd = dee[1];
q1 = p1*dd;
q2 = p2*dd;
ell[1] = q1;
ell[1+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[2];
p2 = ell[2+nskip1];
dd = dee[2];
q1 = p1*dd;
q2 = p2*dd;
ell[2] = q1;
ell[2+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[3];
p2 = ell[3+nskip1];
dd = dee[3];
q1 = p1*dd;
q2 = p2*dd;
ell[3] = q1;
ell[3+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[4];
p2 = ell[4+nskip1];
dd = dee[4];
q1 = p1*dd;
q2 = p2*dd;
ell[4] = q1;
ell[4+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
p1 = ell[5];
p2 = ell[5+nskip1];
dd = dee[5];
q1 = p1*dd;
q2 = p2*dd;
ell[5] = q1;
ell[5+nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
ell += 6;
dee += 6;
}
/* compute left-over iterations */
j += 6;
for (; j > 0; j--) {
p1 = ell[0];
p2 = ell[nskip1];
dd = dee[0];
q1 = p1*dd;
q2 = p2*dd;
ell[0] = q1;
ell[nskip1] = q2;
m11 = p1*q1;
m21 = p2*q1;
m22 = p2*q2;
Z11 += m11;
Z21 += m21;
Z22 += m22;
ell++;
dee++;
}
/* solve for diagonal 2 x 2 block at A(i,i) */
Z11 = ell[0] - Z11;
Z21 = ell[nskip1] - Z21;
Z22 = ell[1+nskip1] - Z22;
dee = d + i;
/* factorize 2 x 2 block Z,dee */
/* factorize row 1 */
dee[0] = dRecip(Z11);
/* factorize row 2 */
sum = 0;
q1 = Z21;
q2 = q1 * dee[0];
Z21 = q2;
sum += q1*q2;
dee[1] = dRecip(Z22 - sum);
/* done factorizing 2 x 2 block */
ell[nskip1] = Z21;
}
/* compute the (less than 2) rows at the bottom */
switch (n-i) {
case 0:
break;
case 1:
dSolveL1_1 (A,A+i*nskip1,i,nskip1);
/* scale the elements in a 1 x i block at A(i,0), and also */
/* compute Z = the outer product matrix that we'll need. */
Z11 = 0;
ell = A+i*nskip1;
dee = d;
for (j=i-6; j >= 0; j -= 6) {
p1 = ell[0];
dd = dee[0];
q1 = p1*dd;
ell[0] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[1];
dd = dee[1];
q1 = p1*dd;
ell[1] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[2];
dd = dee[2];
q1 = p1*dd;
ell[2] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[3];
dd = dee[3];
q1 = p1*dd;
ell[3] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[4];
dd = dee[4];
q1 = p1*dd;
ell[4] = q1;
m11 = p1*q1;
Z11 += m11;
p1 = ell[5];
dd = dee[5];
q1 = p1*dd;
ell[5] = q1;
m11 = p1*q1;
Z11 += m11;
ell += 6;
dee += 6;
}
/* compute left-over iterations */
j += 6;
for (; j > 0; j--) {
p1 = ell[0];
dd = dee[0];
q1 = p1*dd;
ell[0] = q1;
m11 = p1*q1;
Z11 += m11;
ell++;
dee++;
}
/* solve for diagonal 1 x 1 block at A(i,i) */
Z11 = ell[0] - Z11;
dee = d + i;
/* factorize 1 x 1 block Z,dee */
/* factorize row 1 */
dee[0] = dRecip(Z11);
/* done factorizing 1 x 1 block */
break;
default: *((char*)0)=0; /* this should never happen! */
}
}
void
dInternalStepFast (dxWorld * /*world*/, dxBody * body[2], dReal * /*GI*/[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize)
{
int i, j, k;
dReal stepsize1 = dRecip (stepsize);
int m = info.m;
// nothing to do if no constraints.
if (m <= 0)
return;
int nub = 0;
if (info.nub == info.m)
nub = m;
// compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
// format as J so we just go through the constraints in J multiplying by
// the appropriate scalars and matrices.
dReal JinvM[2 * 6 * 8];
//dSetZero (JinvM, 2 * m * 8);
dReal *Jsrc = Jinfo.J1l;
dReal *Jdst = JinvM;
if (body[0])
{
for (j = m - 1; j >= 0; j--)
{
for (k = 0; k < 3; k++)
Jdst[k] = dMUL(Jsrc[k],body[0]->invMass);
dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]);
Jsrc += 8;
Jdst += 8;
}
}
if (body[1])
{
Jsrc = Jinfo.J2l;
Jdst = JinvM + 8 * m;
for (j = m - 1; j >= 0; j--)
{
for (k = 0; k < 3; k++)
Jdst[k] = dMUL(Jsrc[k],body[1]->invMass);
dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]);
Jsrc += 8;
Jdst += 8;
}
}
// now compute A = JinvM * J'.
int mskip = dPAD (m);
dReal A[6 * 8];
//dSetZero (A, 6 * 8);
if (body[0]) {
Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip);
if (body[1])
MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l,
m, mskip);
} else {
if (body[1])
Multiply2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l,
m, mskip);
}
// add cfm to the diagonal of A
for (i = 0; i < m; i++)
A[i * mskip + i] += dMUL(Jinfo.cfm[i],stepsize1);
// compute the right hand side `rhs'
dReal tmp1[16];
//dSetZero (tmp1, 16);
// put v/h + invM*fe into tmp1
for (i = 0; i < 2; i++)
{
if (!body[i])
continue;
for (j = 0; j < 3; j++)
tmp1[i * 8 + j] = dMUL(body[i]->facc[j],body[i]->invMass) + dMUL(body[i]->lvel[j],stepsize1);
dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], body[i]->tacc);
for (j = 0; j < 3; j++)
tmp1[i * 8 + 4 + j] += dMUL(body[i]->avel[j],stepsize1);
}
// put J*tmp1 into rhs
dReal rhs[6];
//dSetZero (rhs, 6);
if (body[0]) {
Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m);
if (body[1])
MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m);
} else {
if (body[1])
Multiply0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m);
}
// complete rhs
for (i = 0; i < m; i++)
rhs[i] = dMUL(Jinfo.c[i],stepsize1) - rhs[i];
#ifdef SLOW_LCP
// solve the LCP problem and get lambda.
// this will destroy A but that's okay
dReal *lambda = (dReal *) ALLOCA (m * sizeof (dReal));
dReal *residual = (dReal *) ALLOCA (m * sizeof (dReal));
dReal lo[6], hi[6];
memcpy (lo, Jinfo.lo, m * sizeof (dReal));
memcpy (hi, Jinfo.hi, m * sizeof (dReal));
dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex);
#endif
// compute the constraint force `cforce'
// compute cforce = J'*lambda
dJointFeedback *fb = joint->feedback;
dReal cforce[16];
//dSetZero (cforce, 16);
if (fb)
{
// the user has requested feedback on the amount of force that this
// joint is applying to the bodies. we use a slightly slower
// computation that splits out the force components and puts them
// in the feedback structure.
dReal data1[8], data2[8];
if (body[0])
{
Multiply1_8q1 (data1, Jinfo.J1l, lambda, m);
dReal *cf1 = cforce;
cf1[0] = (fb->f1[0] = data1[0]);
cf1[1] = (fb->f1[1] = data1[1]);
cf1[2] = (fb->f1[2] = data1[2]);
cf1[4] = (fb->t1[0] = data1[4]);
cf1[5] = (fb->t1[1] = data1[5]);
cf1[6] = (fb->t1[2] = data1[6]);
}
if (body[1])
{
Multiply1_8q1 (data2, Jinfo.J2l, lambda, m);
dReal *cf2 = cforce + 8;
cf2[0] = (fb->f2[0] = data2[0]);
cf2[1] = (fb->f2[1] = data2[1]);
cf2[2] = (fb->f2[2] = data2[2]);
cf2[4] = (fb->t2[0] = data2[4]);
cf2[5] = (fb->t2[1] = data2[5]);
cf2[6] = (fb->t2[2] = data2[6]);
}
}
else
{
// no feedback is required, let's compute cforce the faster way
if (body[0])
Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m);
if (body[1])
Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m);
}
for (i = 0; i < 2; i++)
{
if (!body[i])
continue;
for (j = 0; j < 3; j++)
{
body[i]->facc[j] += cforce[i * 8 + j];
body[i]->tacc[j] += cforce[i * 8 + 4 + j];
}
}
}
EXPORT_C 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 maxc, dContactGeom *contact, int skip)
{
const dReal fudge_factor = REAL(1.05);
dVector3 p,pp,normalC;
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;
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] = dMUL(side1[0],REAL(0.5));
A[1] = dMUL(side1[1],REAL(0.5));
A[2] = dMUL(side1[2],REAL(0.5));
B[0] = dMUL(side2[0],REAL(0.5));
B[1] = dMUL(side2[1],REAL(0.5));
B[2] = dMUL(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.
#define TST(expr1,expr2,norm,cc) \
s2 = dFabs(expr1) - (expr2); \
if (s2 > 0) return 0; \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1) < 0); \
code = (cc); \
}
s = -dInfinity;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + dMUL(B[0],Q11) + dMUL(B[1],Q12) + dMUL(B[2],Q13)),R1+0,1);
TST (pp[1],(A[1] + dMUL(B[0],Q21) + dMUL(B[1],Q22) + dMUL(B[2],Q23)),R1+1,2);
TST (pp[2],(A[2] + dMUL(B[0],Q31) + dMUL(B[1],Q32) + dMUL(B[2],Q33)),R1+2,3);
// separating axis = v1,v2,v3
TST (dDOT41(R2+0,p),(dMUL(A[0],Q11) + dMUL(A[1],Q21) + dMUL(A[2],Q31) + B[0]),R2+0,4);
TST (dDOT41(R2+1,p),(dMUL(A[0],Q12) + dMUL(A[1],Q22) + dMUL(A[2],Q32) + B[1]),R2+1,5);
TST (dDOT41(R2+2,p),(dMUL(A[0],Q13) + dMUL(A[1],Q23) + dMUL(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) \
s2 = dFabs(expr1) - (expr2); \
if (s2 > 0) return 0; \
l = dSqrt (dMUL((n1),(n1)) + dMUL((n2),(n2)) + dMUL((n3),(n3))); \
if (l > 0) { \
s2 = dDIV(s2,l); \
if (dMUL(s2,fudge_factor) > s) { \
s = s2; \
normalR = 0; \
normalC[0] = dDIV((n1),l); normalC[1] = dDIV((n2),l); normalC[2] = dDIV((n3),l); \
invert_normal = ((expr1) < 0); \
code = (cc); \
} \
}
// separating axis = u1 x (v1,v2,v3)
TST((dMUL(pp[2],R21)-dMUL(pp[1],R31)),(dMUL(A[1],Q31)+dMUL(A[2],Q21)+dMUL(B[1],Q13)+dMUL(B[2],Q12)),0,-R31,R21,7);
TST((dMUL(pp[2],R22)-dMUL(pp[1],R32)),(dMUL(A[1],Q32)+dMUL(A[2],Q22)+dMUL(B[0],Q13)+dMUL(B[2],Q11)),0,-R32,R22,8);
TST((dMUL(pp[2],R23)-dMUL(pp[1],R33)),(dMUL(A[1],Q33)+dMUL(A[2],Q23)+dMUL(B[0],Q12)+dMUL(B[1],Q11)),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST((dMUL(pp[0],R31)-dMUL(pp[2],R11)),(dMUL(A[0],Q31)+dMUL(A[2],Q11)+dMUL(B[1],Q23)+dMUL(B[2],Q22)),R31,0,-R11,10);
TST((dMUL(pp[0],R32)-dMUL(pp[2],R12)),(dMUL(A[0],Q32)+dMUL(A[2],Q12)+dMUL(B[0],Q23)+dMUL(B[2],Q21)),R32,0,-R12,11);
TST((dMUL(pp[0],R33)-dMUL(pp[2],R13)),(dMUL(A[0],Q33)+dMUL(A[2],Q13)+dMUL(B[0],Q22)+dMUL(B[1],Q21)),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST((dMUL(pp[1],R11)-dMUL(pp[0],R21)),(dMUL(A[0],Q21)+dMUL(A[1],Q11)+dMUL(B[1],Q33)+dMUL(B[2],Q32)),-R21,R11,0,13);
TST((dMUL(pp[1],R12)-dMUL(pp[0],R22)),(dMUL(A[0],Q22)+dMUL(A[1],Q12)+dMUL(B[0],Q33)+dMUL(B[2],Q31)),-R22,R12,0,14);
TST((dMUL(pp[1],R13)-dMUL(pp[0],R23)),(dMUL(A[0],Q23)+dMUL(A[1],Q13)+dMUL(B[0],Q32)+dMUL(B[1],Q31)),-R23,R13,0,15);
#undef TST
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;
for (i=0; i<3; i++) pa[i] = p1[i];
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] += dMUL(sign,dMUL(A[j],R1[i*4+j]));
}
// find a point pb on the intersecting edge of box 2
dVector3 pb;
for (i=0; i<3; i++) pb[i] = p2[i];
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] += dMUL(sign,dMUL(B[j],R2[i*4+j]));
}
dReal alpha,beta;
dVector3 ua,ub;
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += dMUL(ua[i],alpha);
for (i=0; i<3; i++) pb[i] += dMUL(ub[i],beta);
for (i=0; i<3; i++) contact[0].pos[i] = dMUL(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).
const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) {
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else {
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
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];
}
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 indident face. the other axis numbers of the indicent 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] + dMUL(Sb[lanr],Rb[i*4+lanr]);
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - dMUL(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 = dMUL(m11,Sb[a1]);
dReal k2 = dMUL(m21,Sb[a1]);
dReal k3 = dMUL(m12,Sb[a2]);
dReal k4 = dMUL(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(dMUL(m11,m22) - dMUL(m12,m21));
m11 = dMUL(m11,det1);
m12 = dMUL(m12,det1);
m21 = dMUL(m21,det1);
m22 = dMUL(m22,det1);
int cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
dReal k1 = dMUL(m22,(ret[j*2]-c1)) - dMUL(m12,(ret[j*2+1]-c2));
dReal k2 = -dMUL(m21,(ret[j*2]-c1)) + dMUL(m11,(ret[j*2+1]-c2));
for (i=0; i<3; i++) point[cnum*3+i] =
center[i] + dMUL(k1,Rb[i*4+a1]) + dMUL(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 < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
if (maxc > cnum) maxc = cnum;
if (maxc < 1) maxc = 1;
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 {
// 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
dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations)
{
dxBody *bodyPair[2], *body;
dReal *GIPair[2], *GinvIPair[2];
dxJoint *joint;
int iter, b, j, i;
dReal ministep = stepsize / maxiterations;
// make a local copy of the joint array, because we might want to modify it.
// (the "dxJoint *const*" declaration says we're allowed to modify the joints
// but not the joint array, because the caller might need it unchanged).
dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *));
memcpy (joints, _joints, nj * sizeof (dxJoint *));
// get m = total constraint dimension, nub = number of unbounded variables.
// create constraint offset array and number-of-rows array for all joints.
// the constraints are re-ordered as follows: the purely unbounded
// constraints, the mixed unbounded + LCP constraints, and last the purely
// LCP constraints. this assists the LCP solver to put all unbounded
// variables at the start for a quick factorization.
//
// joints with m=0 are inactive and are removed from the joints array
// entirely, so that the code that follows does not consider them.
// also number all active joints in the joint list (set their tag values).
// inactive joints receive a tag value of -1.
int m = 0;
dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1));
int *ofs = (int *) ALLOCA (nj * sizeof (int));
for (i = 0, j = 0; j < nj; j++)
{ // i=dest, j=src
joints[j]->vtable->getInfo1 (joints[j], info + i);
if (info[i].m > 0)
{
joints[i] = joints[j];
joints[i]->tag = i;
i++;
}
else
{
joints[j]->tag = -1;
}
}
nj = i;
// the purely unbounded constraints
for (i = 0; i < nj; i++)
{
ofs[i] = m;
m += info[i].m;
}
dReal *c = NULL;
dReal *cfm = NULL;
dReal *lo = NULL;
dReal *hi = NULL;
int *findex = NULL;
dReal *J = NULL;
dxJoint::Info2 * Jinfo = NULL;
if (m)
{
// create a constraint equation right hand side vector `c', a constraint
// force mixing vector `cfm', and LCP low and high bound vectors, and an
// 'findex' vector.
c = (dReal *) ALLOCA (m * sizeof (dReal));
cfm = (dReal *) ALLOCA (m * sizeof (dReal));
lo = (dReal *) ALLOCA (m * sizeof (dReal));
hi = (dReal *) ALLOCA (m * sizeof (dReal));
findex = (int *) ALLOCA (m * sizeof (int));
dSetZero (c, m);
dSetValue (cfm, m, world->global_cfm);
dSetValue (lo, m, -dInfinity);
dSetValue (hi, m, dInfinity);
for (i = 0; i < m; i++)
findex[i] = -1;
// get jacobian data from constraints. a (2*m)x8 matrix will be created
// to store the two jacobian blocks from each constraint. it has this
// format:
//
// l l l 0 a a a 0 \ .
// l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows)
// l l l 0 a a a 0 /
// l l l 0 a a a 0 \ .
// l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows)
// l l l 0 a a a 0 /
// l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row)
// l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row)
// etc...
//
// (lll) = linear jacobian data
// (aaa) = angular jacobian data
//
J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal));
dSetZero (J, 2 * m * 8);
Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2));
for (i = 0; i < nj; i++)
{
Jinfo[i].rowskip = 8;
Jinfo[i].fps = dRecip (stepsize);
Jinfo[i].erp = world->global_erp;
Jinfo[i].J1l = J + 2 * 8 * ofs[i];
Jinfo[i].J1a = Jinfo[i].J1l + 4;
Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m;
Jinfo[i].J2a = Jinfo[i].J2l + 4;
Jinfo[i].c = c + ofs[i];
Jinfo[i].cfm = cfm + ofs[i];
Jinfo[i].lo = lo + ofs[i];
Jinfo[i].hi = hi + ofs[i];
Jinfo[i].findex = findex + ofs[i];
//joints[i]->vtable->getInfo2 (joints[i], Jinfo+i);
}
}
dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal));
dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal));
dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
for (b = 0; b < nb; b++)
{
for (i = 0; i < 4; i++)
{
saveFacc[b * 4 + i] = bodies[b]->facc[i];
saveTacc[b * 4 + i] = bodies[b]->tacc[i];
}
bodies[b]->tag = b;
}
for (iter = 0; iter < maxiterations; iter++)
{
dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
for (b = 0; b < nb; b++)
{
body = bodies[b];
// for all bodies, compute the inertia tensor and its inverse in the global
// frame, and compute the rotational force and add it to the torque
// accumulator. I and invI are vertically stacked 3x4 matrices, one per body.
// @@@ check computation of rotational force.
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp, body->mass.I, body->posr.R);
dMULTIPLY0_333 (globalI + b * 12, body->posr.R, tmp);
// compute inverse inertia tensor in global frame
dMULTIPLY2_333 (tmp, body->invI, body->posr.R);
dMULTIPLY0_333 (globalInvI + b * 12, body->posr.R, tmp);
for (i = 0; i < 4; i++)
body->tacc[i] = saveTacc[b * 4 + i];
// add the gravity force to all bodies
if ((body->flags & dxBodyNoGravity) == 0)
{
body->facc[0] = saveFacc[b * 4 + 0] + dMUL(body->mass.mass,world->gravity[0]);
body->facc[1] = saveFacc[b * 4 + 1] + dMUL(body->mass.mass,world->gravity[1]);
body->facc[2] = saveFacc[b * 4 + 2] + dMUL(body->mass.mass,world->gravity[2]);
body->facc[3] = 0;
} else {
body->facc[0] = saveFacc[b * 4 + 0];
body->facc[1] = saveFacc[b * 4 + 1];
body->facc[2] = saveFacc[b * 4 + 2];
body->facc[3] = 0;
}
}
#ifdef RANDOM_JOINT_ORDER
//randomize the order of the joints by looping through the array
//and swapping the current joint pointer with a random one before it.
for (j = 0; j < nj; j++)
{
joint = joints[j];
dxJoint::Info1 i1 = info[j];
dxJoint::Info2 i2 = Jinfo[j];
const int r = dRandInt(j+1);
joints[j] = joints[r];
info[j] = info[r];
Jinfo[j] = Jinfo[r];
joints[r] = joint;
info[r] = i1;
Jinfo[r] = i2;
}
#endif
//now iterate through the random ordered joint array we created.
for (j = 0; j < nj; j++)
{
joint = joints[j];
bodyPair[0] = joint->node[0].body;
bodyPair[1] = joint->node[1].body;
if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled))
bodyPair[0] = 0;
if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled))
bodyPair[1] = 0;
//if this joint is not connected to any enabled bodies, skip it.
if (!bodyPair[0] && !bodyPair[1])
continue;
if (bodyPair[0])
{
GIPair[0] = globalI + bodyPair[0]->tag * 12;
GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12;
}
if (bodyPair[1])
{
GIPair[1] = globalI + bodyPair[1]->tag * 12;
GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12;
}
joints[j]->vtable->getInfo2 (joints[j], Jinfo + j);
//dInternalStepIslandFast is an exact copy of the old routine with one
//modification: the calculated forces are added back to the facc and tacc
//vectors instead of applying them to the bodies and moving them.
if (info[j].m > 0)
{
dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep);
}
}
// }
//Now we can simulate all the free floating bodies, and move them.
for (b = 0; b < nb; b++)
{
body = bodies[b];
for (i = 0; i < 4; i++)
{
body->facc[i] = dMUL(body->facc[i],ministep);
body->tacc[i] = dMUL(body->tacc[i],ministep);
}
//apply torque
dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc);
//apply force
for (i = 0; i < 3; i++)
body->lvel[i] += dMUL(body->invMass,body->facc[i]);
//move It!
moveAndRotateBody (body, ministep);
}
}
for (b = 0; b < nb; b++)
for (j = 0; j < 4; j++)
bodies[b]->facc[j] = bodies[b]->tacc[j] = 0;
}
