// test one mesh triangle on intersection with capsule
void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule(
const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
uint8 flags)
{
// calculate edges
SUBTRACT(v1,v0,m_vE0);
SUBTRACT(v2,v1,m_vE1);
SUBTRACT(v0,v2,m_vE2);
dVector3 _minus_vE0;
SUBTRACT(v0,v1,_minus_vE0);
// calculate poly normal
dCalcVectorCross3(m_vN,m_vE1,_minus_vE0);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(m_vN))
{
return;
}
// create plane from triangle
dReal plDistance = -dCalcVectorDot3(v0,m_vN);
dVector4 plTrianglePlane;
CONSTRUCTPLANE(plTrianglePlane,m_vN,plDistance);
// calculate capsule distance to plane
dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,m_vCapsulePosition);
// Capsule must be over positive side of triangle
if (fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/)
{
// if not don't generate contacts
return;
}
dVector3 vPnt0;
SET (vPnt0,v0);
dVector3 vPnt1;
SET (vPnt1,v1);
dVector3 vPnt2;
SET (vPnt2,v2);
if (fDistanceCapsuleCenterToPlane < 0 )
{
SET (vPnt0,v0);
SET (vPnt1,v2);
SET (vPnt2,v1);
}
// do intersection test and find best separating axis
if (!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags))
{
// if not found do nothing
return;
}
// if best separation axis is not found
if (m_iBestAxis == 0 )
{
// this should not happen (we should already exit in that case)
dIASSERT(FALSE);
// do nothing
return;
}
// calculate caps centers in absolute space
dVector3 vCposTrans;
vCposTrans[0] = m_vCapsulePosition[0] + m_vNormal[0]*m_vCapsuleRadius;
vCposTrans[1] = m_vCapsulePosition[1] + m_vNormal[1]*m_vCapsuleRadius;
vCposTrans[2] = m_vCapsulePosition[2] + m_vNormal[2]*m_vCapsuleRadius;
dVector3 vCEdgePoint0;
vCEdgePoint0[0] = vCposTrans[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint0[1] = vCposTrans[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint0[2] = vCposTrans[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
dVector3 vCEdgePoint1;
vCEdgePoint1[0] = vCposTrans[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint1[1] = vCposTrans[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint1[2] = vCposTrans[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
// transform capsule edge points into triangle space
vCEdgePoint0[0] -= vPnt0[0];
vCEdgePoint0[1] -= vPnt0[1];
vCEdgePoint0[2] -= vPnt0[2];
vCEdgePoint1[0] -= vPnt0[0];
vCEdgePoint1[1] -= vPnt0[1];
vCEdgePoint1[2] -= vPnt0[2];
dVector4 plPlane;
dVector3 _minus_vN;
_minus_vN[0] = -m_vN[0];
_minus_vN[1] = -m_vN[1];
_minus_vN[2] = -m_vN[2];
// triangle plane
CONSTRUCTPLANE(plPlane,_minus_vN,0);
//plPlane = Plane4f( -m_vN, 0);
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
// plane with edge 0
dVector3 vTemp;
dCalcVectorCross3(vTemp,m_vN,m_vE0);
CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
dCalcVectorCross3(vTemp,m_vN,m_vE1);
CONSTRUCTPLANE(plPlane, vTemp, -(dCalcVectorDot3(m_vE0,vTemp)-REAL(1e-5)));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
dCalcVectorCross3(vTemp,m_vN,m_vE2);
CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) {
return;
}
// return capsule edge points into absolute space
vCEdgePoint0[0] += vPnt0[0];
vCEdgePoint0[1] += vPnt0[1];
vCEdgePoint0[2] += vPnt0[2];
vCEdgePoint1[0] += vPnt0[0];
vCEdgePoint1[1] += vPnt0[1];
vCEdgePoint1[2] += vPnt0[2];
// calculate depths for both contact points
SUBTRACT(vCEdgePoint0,m_vCapsulePosition,vTemp);
dReal fDepth0 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt);
SUBTRACT(vCEdgePoint1,m_vCapsulePosition,vTemp);
dReal fDepth1 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt);
// clamp depths to zero
if (fDepth0 < 0)
{
fDepth0 = 0.0f;
}
if (fDepth1 < 0 )
{
fDepth1 = 0.0f;
}
// Cached contacts's data
// contact 0
dIASSERT(m_ctContacts < (m_iFlags & NUMC_MASK)); // Do not call function if there is no room to store result
m_gLocalContacts[m_ctContacts].fDepth = fDepth0;
SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal);
SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint0);
m_gLocalContacts[m_ctContacts].nFlags = 1;
m_ctContacts++;
if (m_ctContacts < (m_iFlags & NUMC_MASK)) {
// contact 1
m_gLocalContacts[m_ctContacts].fDepth = fDepth1;
SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal);
SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint1);
m_gLocalContacts[m_ctContacts].nFlags = 1;
m_ctContacts++;
}
}
// clip and generate contacts
void sTrimeshBoxColliderData::_cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, int TriIndex) {
dIASSERT( !(m_iFlags & CONTACTS_UNIMPORTANT) || m_ctContacts < (m_iFlags & NUMC_MASK) ); // Do not call the function if there is no room to store results
// if we have edge/edge intersection
if (m_iBestAxis > 4 ) {
dVector3 vub,vPb,vPa;
SET(vPa,m_vHullBoxPos);
// calculate point on box edge
for( int i=0; i<3; i++) {
dVector3 vRotCol;
GETCOL(m_mHullBoxRot,i,vRotCol);
dReal fSign = dDOT(m_vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f;
vPa[0] += fSign * m_vBoxHalfSize[i] * vRotCol[0];
vPa[1] += fSign * m_vBoxHalfSize[i] * vRotCol[1];
vPa[2] += fSign * m_vBoxHalfSize[i] * vRotCol[2];
}
int iEdge = (m_iBestAxis-5)%3;
// decide which edge is on triangle
if ( iEdge == 0 ) {
SET(vPb,v0);
SET(vub,m_vE0);
} else if ( iEdge == 1) {
SET(vPb,v2);
SET(vub,m_vE1);
} else {
SET(vPb,v1);
SET(vub,m_vE2);
}
// setup direction parameter for face edge
dNormalize3(vub);
dReal fParam1, fParam2;
// setup direction parameter for box edge
dVector3 vua;
int col=(m_iBestAxis-5)/3;
GETCOL(m_mHullBoxRot,col,vua);
// find two closest points on both edges
_cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 );
vPa[0] += vua[0]*fParam1;
vPa[1] += vua[1]*fParam1;
vPa[2] += vua[2]*fParam1;
vPb[0] += vub[0]*fParam2;
vPb[1] += vub[1]*fParam2;
vPb[2] += vub[2]*fParam2;
// calculate collision point
dVector3 vPntTmp;
ADD(vPa,vPb,vPntTmp);
vPntTmp[0]*=0.5f;
vPntTmp[1]*=0.5f;
vPntTmp[2]*=0.5f;
// generate contact point between two closest points
#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
dContactGeom* Contact = SAFECONTACT(m_iFlags, m_ContactGeoms, m_ctContacts, m_iStride);
Contact->depth = m_fBestDepth;
SET(Contact->normal,m_vBestNormal);
SET(Contact->pos,vPntTmp);
Contact->g1 = Geom1;
Contact->g2 = Geom2;
Contact->side1 = TriIndex;
Contact->side2 = -1;
m_ctContacts++;
#endif
GenerateContact(m_iFlags, m_ContactGeoms, m_iStride, m_Geom1, m_Geom2, TriIndex,
vPntTmp, m_vBestNormal, m_fBestDepth, m_ctContacts);
// if triangle is the referent face then clip box to triangle face
} else if (m_iBestAxis == 1) {
dVector3 vNormal2;
vNormal2[0]=-m_vBestNormal[0];
vNormal2[1]=-m_vBestNormal[1];
vNormal2[2]=-m_vBestNormal[2];
// vNr is normal in box frame, pointing from triangle to box
dMatrix3 mTransposed;
mTransposed[0*4+0]=m_mHullBoxRot[0*4+0];
mTransposed[0*4+1]=m_mHullBoxRot[1*4+0];
mTransposed[0*4+2]=m_mHullBoxRot[2*4+0];
mTransposed[1*4+0]=m_mHullBoxRot[0*4+1];
mTransposed[1*4+1]=m_mHullBoxRot[1*4+1];
mTransposed[1*4+2]=m_mHullBoxRot[2*4+1];
mTransposed[2*4+0]=m_mHullBoxRot[0*4+2];
mTransposed[2*4+1]=m_mHullBoxRot[1*4+2];
mTransposed[2*4+2]=m_mHullBoxRot[2*4+2];
dVector3 vNr;
vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2];
vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2];
vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2];
dVector3 vAbsNormal;
vAbsNormal[0] = dFabs( vNr[0] );
vAbsNormal[1] = dFabs( vNr[1] );
vAbsNormal[2] = dFabs( vNr[2] );
// get closest face from box
int iB0, iB1, iB2;
if (vAbsNormal[1] > vAbsNormal[0]) {
if (vAbsNormal[1] > vAbsNormal[2]) {
iB1 = 0; iB0 = 1; iB2 = 2;
} else {
iB1 = 0; iB2 = 1; iB0 = 2;
}
} else {
if (vAbsNormal[0] > vAbsNormal[2]) {
iB0 = 0; iB1 = 1; iB2 = 2;
} else {
iB1 = 0; iB2 = 1; iB0 = 2;
}
}
// Here find center of box face we are going to project
dVector3 vCenter;
dVector3 vRotCol;
GETCOL(m_mHullBoxRot,iB0,vRotCol);
if (vNr[iB0] > 0) {
vCenter[0] = m_vHullBoxPos[0] - v0[0] - m_vBoxHalfSize[iB0] * vRotCol[0];
vCenter[1] = m_vHullBoxPos[1] - v0[1] - m_vBoxHalfSize[iB0] * vRotCol[1];
vCenter[2] = m_vHullBoxPos[2] - v0[2] - m_vBoxHalfSize[iB0] * vRotCol[2];
} else {
vCenter[0] = m_vHullBoxPos[0] - v0[0] + m_vBoxHalfSize[iB0] * vRotCol[0];
vCenter[1] = m_vHullBoxPos[1] - v0[1] + m_vBoxHalfSize[iB0] * vRotCol[1];
vCenter[2] = m_vHullBoxPos[2] - v0[2] + m_vBoxHalfSize[iB0] * vRotCol[2];
}
// Here find 4 corner points of box
dVector3 avPoints[4];
dVector3 vRotCol2;
GETCOL(m_mHullBoxRot,iB1,vRotCol);
GETCOL(m_mHullBoxRot,iB2,vRotCol2);
for(int x=0;x<3;x++) {
avPoints[0][x] = vCenter[x] + (m_vBoxHalfSize[iB1] * vRotCol[x]) - (m_vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[1][x] = vCenter[x] - (m_vBoxHalfSize[iB1] * vRotCol[x]) - (m_vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[2][x] = vCenter[x] - (m_vBoxHalfSize[iB1] * vRotCol[x]) + (m_vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[3][x] = vCenter[x] + (m_vBoxHalfSize[iB1] * vRotCol[x]) + (m_vBoxHalfSize[iB2] * vRotCol2[x]);
}
// clip Box face with 4 planes of triangle (1 face plane, 3 egde planes)
dVector3 avTempArray1[9];
dVector3 avTempArray2[9];
dVector4 plPlane;
int iTempCnt1=0;
int iTempCnt2=0;
// zeroify vectors - necessary?
for(int i=0; i<9; i++) {
avTempArray1[i][0]=0;
avTempArray1[i][1]=0;
avTempArray1[i][2]=0;
avTempArray2[i][0]=0;
avTempArray2[i][1]=0;
avTempArray2[i][2]=0;
}
// Normal plane
dVector3 vTemp;
vTemp[0]=-m_vN[0];
vTemp[1]=-m_vN[1];
vTemp[2]=-m_vN[2];
dNormalize3(vTemp);
CONSTRUCTPLANE(plPlane,vTemp,0);
_cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane );
// Plane p0
dVector3 vTemp2;
SUBTRACT(v1,v0,vTemp2);
dCROSS(vTemp,=,m_vN,vTemp2);
dNormalize3(vTemp);
CONSTRUCTPLANE(plPlane,vTemp,0);
_cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
// Plane p1
SUBTRACT(v2,v1,vTemp2);
dCROSS(vTemp,=,m_vN,vTemp2);
dNormalize3(vTemp);
SUBTRACT(v0,v2,vTemp2);
CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp));
_cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane );
// Plane p2
SUBTRACT(v0,v2,vTemp2);
dCROSS(vTemp,=,m_vN,vTemp2);
dNormalize3(vTemp);
CONSTRUCTPLANE(plPlane,vTemp,0);
_cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
// END of clipping polygons
// for each generated contact point
for ( int i=0; i<iTempCnt2; i++ ) {
// calculate depth
dReal fTempDepth = dDOT(vNormal2,avTempArray2[i]);
// clamp depth to zero
if (fTempDepth > 0) {
fTempDepth = 0;
}
dVector3 vPntTmp;
ADD(avTempArray2[i],v0,vPntTmp);
#if 0 //#ifdef ORIG -- if to use conditional define, GenerateContact must be moved into #else
dContactGeom* Contact = SAFECONTACT(m_iFlags, m_ContactGeoms, m_ctContacts, m_iStride);
Contact->depth = -fTempDepth;
SET(Contact->normal,m_vBestNormal);
SET(Contact->pos,vPntTmp);
Contact->g1 = Geom1;
Contact->g2 = Geom2;
Contact->side1 = TriIndex;
Contact->side2 = -1;
m_ctContacts++;
#endif
GenerateContact(m_iFlags, m_ContactGeoms, m_iStride, m_Geom1, m_Geom2, TriIndex,
vPntTmp, m_vBestNormal, -fTempDepth, m_ctContacts);
if ((m_ctContacts | CONTACTS_UNIMPORTANT) == (m_iFlags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
break;
}
}
//dAASSERT(m_ctContacts>0);
// if box face is the referent face, then clip triangle on box face
} else { // 2 <= if iBestAxis <= 4
// capsule - trimesh by CroTeam
// Ported by Nguyem Binh
int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dTriMeshClass);
dIASSERT (o2->type == dCapsuleClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
int nContactCount = 0;
dxTriMesh *TriMesh = (dxTriMesh*)o1;
dxGeom *Capsule = o2;
sTrimeshCapsuleColliderData cData;
cData.SetupInitialContext(TriMesh, Capsule, flags, skip);
const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == Capsule->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
OBBCollider& Collider = pccColliderCache->_OBBCollider;
// Will it better to use LSS here? -> confirm Pierre.
dQueryCCTLPotentialCollisionTriangles(Collider, cData,
TriMesh, Capsule, pccColliderCache->defaultBoxCache);
if (Collider.GetContactStatus())
{
// Retrieve data
int TriCount = Collider.GetNbTouchedPrimitives();
if (TriCount != 0)
{
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriMesh->ArrayCallback != null)
{
TriMesh->ArrayCallback(TriMesh, Capsule, Triangles, TriCount);
}
// allocate buffer for local contacts on stack
cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK));
unsigned int ctContacts0 = cData.m_ctContacts;
uint8* UseFlags = TriMesh->Data->UseFlags;
// loop through all intersecting triangles
for (int i = 0; i < TriCount; i++)
{
const int Triint = Triangles[i];
if (!Callback(TriMesh, Capsule, Triint)) continue;
dVector3 dv[3];
FetchTriangle(TriMesh, Triint, cData.m_mTriMeshPos, cData.m_mTriMeshRot, dv);
uint8 flags = UseFlags ? UseFlags[Triint] : dxTriMeshData::kUseAll;
bool bFinishSearching;
ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, flags, bFinishSearching);
if (bFinishSearching)
{
break;
}
}
if (cData.m_ctContacts != 0)
{
nContactCount = cData._ProcessLocalContacts(contact, TriMesh, Capsule);
}
}
}
return nContactCount;
}
// capsule - trimesh By francisco leon
int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dTriMeshClass);
dIASSERT (o2->type == dCapsuleClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)o1;
dxGeom* gCylinder = o2;
//Get capsule params
dMatrix3 mCapsuleRotation;
dVector3 vCapsulePosition;
dVector3 vCapsuleAxis;
dReal vCapsuleRadius;
dReal fCapsuleSize;
dMatrix3* pRot = (dMatrix3*) dGeomGetRotation(gCylinder);
memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
dVector3* pDst = (dVector3*)dGeomGetPosition(gCylinder);
memcpy(vCapsulePosition,pDst,sizeof(dVector3));
//Axis
vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS];
vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
// Get size of CCylinder
dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
fCapsuleSize*=0.5f;
//Set Capsule params
GIM_CAPSULE_DATA capsule;
capsule.m_radius = vCapsuleRadius;
VEC_SCALE(capsule.m_point1,fCapsuleSize,vCapsuleAxis);
VEC_SUM(capsule.m_point1,vCapsulePosition,capsule.m_point1);
VEC_SCALE(capsule.m_point2,-fCapsuleSize,vCapsuleAxis);
VEC_SUM(capsule.m_point2,vCapsulePosition,capsule.m_point2);
//Create contact list
GDYNAMIC_ARRAY trimeshcontacts;
GIM_CREATE_CONTACT_LIST(trimeshcontacts);
//Collide trimeshe vs capsule
gim_trimesh_capsule_collision(&TriMesh->m_collision_trimesh,&capsule,&trimeshcontacts);
if(trimeshcontacts.m_size == 0)
{
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return 0;
}
GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
unsigned contactcount = trimeshcontacts.m_size;
unsigned contactmax = (unsigned)(flags & NUMC_MASK);
if (contactcount > contactmax)
{
contactcount = contactmax;
}
dContactGeom* pcontact;
unsigned i;
for (i=0;i<contactcount;i++)
{
pcontact = SAFECONTACT(flags, contact, i, skip);
pcontact->pos[0] = ptrimeshcontacts->m_point[0];
pcontact->pos[1] = ptrimeshcontacts->m_point[1];
pcontact->pos[2] = ptrimeshcontacts->m_point[2];
pcontact->pos[3] = 1.0f;
pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
pcontact->normal[3] = 0;
pcontact->depth = ptrimeshcontacts->m_depth;
pcontact->g1 = TriMesh;
pcontact->g2 = gCylinder;
pcontact->side1 = ptrimeshcontacts->m_feature1;
pcontact->side2 = -1;
ptrimeshcontacts++;
}
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return (int)contactcount;
}
// 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 = dDOT41( 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 = dDOT( q, q ) - ( cyl->radius * cyl->radius );
// Compute the projection of ray direction normal onto cylinder direction normal.
dReal uv = dDOT44( 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 = dDOT( r, r );
dReal B = 2 * dDOT( 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 = dDOT14( 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;
}
void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper)
{
dxBody *b,*bb,**body;
dxJoint *j,**joint;
// nothing to do if no bodies
if (world->nb <= 0) return;
// handle auto-disabling of bodies
dInternalHandleAutoDisabling (world,stepsize);
// make arrays for body and joint lists (for a single island) to go into
body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*));
joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*));
int bcount = 0; // number of bodies in `body'
int jcount = 0; // number of joints in `joint'
// set all body/joint tags to 0
for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0;
for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0;
// allocate a stack of unvisited bodies in the island. the maximum size of
// the stack can be the lesser of the number of bodies or joints, because
// new bodies are only ever added to the stack by going through untagged
// joints. all the bodies in the stack must be tagged!
int stackalloc = (world->nj < world->nb) ? world->nj : world->nb;
dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*));
for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
// get bb = the next enabled, untagged body, and tag it
if (bb->tag || (bb->flags & dxBodyDisabled)) continue;
bb->tag = 1;
// tag all bodies and joints starting from bb.
int stacksize = 0;
b = bb;
body[0] = bb;
bcount = 1;
jcount = 0;
goto quickstart;
while (stacksize > 0) {
b = stack[--stacksize]; // pop body off stack
body[bcount++] = b; // put body on body list
quickstart:
// traverse and tag all body's joints, add untagged connected bodies
// to stack
for (dxJointNode *n=b->firstjoint; n; n=n->next) {
if (!n->joint->tag) {
n->joint->tag = 1;
joint[jcount++] = n->joint;
if (n->body && !n->body->tag) {
n->body->tag = 1;
stack[stacksize++] = n->body;
}
}
}
dIASSERT(stacksize <= world->nb);
dIASSERT(stacksize <= world->nj);
}
// now do something with body and joint lists
stepper (world,body,bcount,joint,jcount,stepsize);
// what we've just done may have altered the body/joint tag values.
// we must make sure that these tags are nonzero.
// also make sure all bodies are in the enabled state.
int i;
for (i=0; i<bcount; i++) {
body[i]->tag = 1;
body[i]->flags &= ~dxBodyDisabled;
}
for (i=0; i<jcount; i++) joint[i]->tag = 1;
}
// if debugging, check that all objects (except for disabled bodies,
// unconnected joints, and joints that are connected to disabled bodies)
// were tagged.
# ifndef dNODEBUG
for (b=world->firstbody; b; b=(dxBody*)b->next) {
if (b->flags & dxBodyDisabled) {
if (b->tag) dDebug (0,"disabled body tagged");
}
else {
if (!b->tag) dDebug (0,"enabled body not tagged");
}
}
for (j=world->firstjoint; j; j=(dxJoint*)j->next) {
if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) ||
(j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) {
if (!j->tag) dDebug (0,"attached enabled joint not tagged");
}
else {
if (j->tag) dDebug (0,"unattached or disabled joint tagged");
}
}
# endif
}
int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dRayClass);
dIASSERT (o2->type == dBoxClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxRay *ray = (dxRay*) o1;
dxBox *box = (dxBox*) o2;
contact->g1 = ray;
contact->g2 = box;
contact->side1 = -1;
contact->side2 = -1;
int i;
// compute the start and delta of the ray relative to the box.
// we will do all subsequent computations in this box-relative coordinate
// system. we have to do a translation and rotation for each point.
dVector3 tmp,s,v;
tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0];
tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1];
tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2];
dMULTIPLY1_331 (s,box->final_posr->R,tmp);
tmp[0] = ray->final_posr->R[0*4+2];
tmp[1] = ray->final_posr->R[1*4+2];
tmp[2] = ray->final_posr->R[2*4+2];
dMULTIPLY1_331 (v,box->final_posr->R,tmp);
// mirror the line so that v has all components >= 0
dVector3 sign;
for (i=0; i<3; i++) {
if (v[i] < 0) {
s[i] = -s[i];
v[i] = -v[i];
sign[i] = 1;
}
else sign[i] = -1;
}
// compute the half-sides of the box
dReal h[3];
h[0] = REAL(0.5) * box->side[0];
h[1] = REAL(0.5) * box->side[1];
h[2] = REAL(0.5) * box->side[2];
// do a few early exit tests
if ((s[0] < -h[0] && v[0] <= 0) || s[0] > h[0] ||
(s[1] < -h[1] && v[1] <= 0) || s[1] > h[1] ||
(s[2] < -h[2] && v[2] <= 0) || s[2] > h[2] ||
(v[0] == 0 && v[1] == 0 && v[2] == 0)) {
return 0;
}
// compute the t=[lo..hi] range for where s+v*t intersects the box
dReal lo = -dInfinity;
dReal hi = dInfinity;
int nlo = 0, nhi = 0;
for (i=0; i<3; i++) {
if (v[i] != 0) {
dReal k = (-h[i] - s[i])/v[i];
if (k > lo) {
lo = k;
nlo = i;
}
k = (h[i] - s[i])/v[i];
if (k < hi) {
hi = k;
nhi = i;
}
}
}
// check if the ray intersects
if (lo > hi) return 0;
dReal alpha;
int n;
if (lo >= 0) {
alpha = lo;
n = nlo;
}
else {
alpha = hi;
n = nhi;
}
if (alpha < 0 || alpha > ray->length) return 0;
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];
contact->normal[0] = box->final_posr->R[0*4+n] * sign[n];
contact->normal[1] = box->final_posr->R[1*4+n] * sign[n];
contact->normal[2] = box->final_posr->R[2*4+n] * sign[n];
contact->depth = alpha;
return 1;
}
int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dCapsuleClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
int i;
const dReal tolerance = REAL(1e-5);
dxCapsule *cyl1 = (dxCapsule*) o1;
dxCapsule *cyl2 = (dxCapsule*) o2;
contact->g1 = o1;
contact->g2 = o2;
contact->side1 = -1;
contact->side2 = -1;
// copy out some variables, for convenience
dReal lz1 = cyl1->lz * REAL(0.5);
dReal lz2 = cyl2->lz * REAL(0.5);
dReal *pos1 = o1->final_posr->pos;
dReal *pos2 = o2->final_posr->pos;
dReal axis1[3],axis2[3];
axis1[0] = o1->final_posr->R[2];
axis1[1] = o1->final_posr->R[6];
axis1[2] = o1->final_posr->R[10];
axis2[0] = o2->final_posr->R[2];
axis2[1] = o2->final_posr->R[6];
axis2[2] = o2->final_posr->R[10];
// if the cylinder axes are close to parallel, we'll try to detect up to
// two contact points along the body of the cylinder. if we can't find any
// points then we'll fall back to the closest-points algorithm. note that
// we are not treating this special case for reasons of degeneracy, but
// because we want two contact points in some situations. the closet-points
// algorithm is robust in all casts, but it can return only one contact.
dVector3 sphere1,sphere2;
dReal a1a2 = dCalcVectorDot3 (axis1,axis2);
dReal det = REAL(1.0)-a1a2*a1a2;
if (det < tolerance) {
// the cylinder axes (almost) parallel, so we will generate up to two
// contacts. alpha1 and alpha2 (line position parameters) are related by:
// alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2)
// or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2)
// first compute where the two cylinders overlap in alpha1 space:
if (a1a2 < 0) {
axis2[0] = -axis2[0];
axis2[1] = -axis2[1];
axis2[2] = -axis2[2];
}
dReal q[3];
for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i];
dReal k = dCalcVectorDot3 (axis1,q);
dReal a1lo = -lz1;
dReal a1hi = lz1;
dReal a2lo = -lz2 - k;
dReal a2hi = lz2 - k;
dReal lo = (a1lo > a2lo) ? a1lo : a2lo;
dReal hi = (a1hi < a2hi) ? a1hi : a2hi;
if (lo <= hi) {
int num_contacts = flags & NUMC_MASK;
if (num_contacts >= 2 && lo < hi) {
// generate up to two contacts. if one of those contacts is
// not made, fall back on the one-contact strategy.
for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i];
for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i];
int n1 = dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius,contact);
if (n1) {
for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i];
for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i];
dContactGeom *c2 = CONTACT(contact,skip);
int n2 = dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius, c2);
if (n2) {
c2->g1 = o1;
c2->g2 = o2;
c2->side1 = -1;
c2->side2 = -1;
return 2;
}
}
}
// just one contact to generate, so put it in the middle of
// the range
dReal alpha1 = (lo + hi) * REAL(0.5);
dReal alpha2 = alpha1 + k;
for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
return dCollideSpheres (sphere1,cyl1->radius,
sphere2,cyl2->radius,contact);
}
}
// use the closest point algorithm
dVector3 a1,a2,b1,b2;
a1[0] = o1->final_posr->pos[0] + axis1[0]*lz1;
a1[1] = o1->final_posr->pos[1] + axis1[1]*lz1;
a1[2] = o1->final_posr->pos[2] + axis1[2]*lz1;
a2[0] = o1->final_posr->pos[0] - axis1[0]*lz1;
a2[1] = o1->final_posr->pos[1] - axis1[1]*lz1;
a2[2] = o1->final_posr->pos[2] - axis1[2]*lz1;
b1[0] = o2->final_posr->pos[0] + axis2[0]*lz2;
b1[1] = o2->final_posr->pos[1] + axis2[1]*lz2;
b1[2] = o2->final_posr->pos[2] + axis2[2]*lz2;
b2[0] = o2->final_posr->pos[0] - axis2[0]*lz2;
b2[1] = o2->final_posr->pos[1] - axis2[1]*lz2;
b2[2] = o2->final_posr->pos[2] - axis2[2]*lz2;
dClosestLineSegmentPoints (a1,a2,b1,b2,sphere1,sphere2);
return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact);
}
void dSolveLCP (dxWorldProcessMemArena *memarena, int n, dReal *A, dReal *x, dReal *b,
dReal *outer_w/*=NULL*/, int nub, dReal *lo, dReal *hi, int *findex)
{
dAASSERT (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
# ifndef dNODEBUG
{
// check restrictions on lo and hi
for (int k=0; k<n; ++k) dIASSERT (lo[k] <= 0 && hi[k] >= 0);
}
# endif
// if all the variables are unbounded then we can just factor, solve,
// and return
if (nub >= n) {
dReal *d = memarena->AllocateArray<dReal> (n);
dSetZero (d, n);
int nskip = dPAD(n);
dFactorLDLT (A, d, n, nskip);
dSolveLDLT (A, d, b, n, nskip);
memcpy (x, b, n*sizeof(dReal));
return;
}
const int nskip = dPAD(n);
dReal *L = memarena->AllocateArray<dReal> (n*nskip);
dReal *d = memarena->AllocateArray<dReal> (n);
dReal *w = outer_w ? outer_w : memarena->AllocateArray<dReal> (n);
dReal *delta_w = memarena->AllocateArray<dReal> (n);
dReal *delta_x = memarena->AllocateArray<dReal> (n);
dReal *Dell = memarena->AllocateArray<dReal> (n);
dReal *ell = memarena->AllocateArray<dReal> (n);
#ifdef ROWPTRS
dReal **Arows = memarena->AllocateArray<dReal *> (n);
#else
dReal **Arows = NULL;
#endif
int *p = memarena->AllocateArray<int> (n);
int *C = memarena->AllocateArray<int> (n);
// for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
bool *state = memarena->AllocateArray<bool> (n);
// create LCP object. note that tmp is set to delta_w to save space, this
// optimization relies on knowledge of how tmp is used, so be careful!
dLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows);
int adj_nub = lcp.getNub();
// loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the
// LCP conditions then i is added to the appropriate index set. otherwise
// x(i),w(i) is driven either +ve or -ve to force it to the valid region.
// as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
// while driving x(i) we maintain the LCP conditions on the other variables
// 0..i-1. we do this by watching out for other x(i),w(i) values going
// outside the valid region, and then switching them between index sets
// when that happens.
bool hit_first_friction_index = false;
for (int i=adj_nub; i<n; ++i) {
bool s_error = false;
// the index i is the driving index and indexes i+1..n-1 are "dont care",
// i.e. when we make changes to the system those x's will be zero and we
// don't care what happens to those w's. in other words, we only consider
// an (i+1)*(i+1) sub-problem of A*x=b+w.
// if we've hit the first friction index, we have to compute the lo and
// hi values based on the values of x already computed. we have been
// permuting the indexes, so the values stored in the findex vector are
// no longer valid. thus we have to temporarily unpermute the x vector.
// for the purposes of this computation, 0*infinity = 0 ... so if the
// contact constraint's normal force is 0, there should be no tangential
// force applied.
if (!hit_first_friction_index && findex && findex[i] >= 0) {
// un-permute x into delta_w, which is not being used at the moment
for (int j=0; j<n; ++j) delta_w[p[j]] = x[j];
// set lo and hi values
for (int k=i; k<n; ++k) {
dReal wfk = delta_w[findex[k]];
if (wfk == 0) {
hi[k] = 0;
lo[k] = 0;
}
else {
hi[k] = dFabs (hi[k] * wfk);
lo[k] = -hi[k];
}
}
hit_first_friction_index = true;
}
// thus far we have not even been computing the w values for indexes
// greater than i, so compute w[i] now.
w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i];
// if lo=hi=0 (which can happen for tangential friction when normals are
// 0) then the index will be assigned to set N with some state. however,
// set C's line has zero size, so the index will always remain in set N.
// with the "normal" switching logic, if w changed sign then the index
// would have to switch to set C and then back to set N with an inverted
// state. this is pointless, and also computationally expensive. to
// prevent this from happening, we use the rule that indexes with lo=hi=0
// will never be checked for set changes. this means that the state for
// these indexes may be incorrect, but that doesn't matter.
// see if x(i),w(i) is in a valid region
if (lo[i]==0 && w[i] >= 0) {
lcp.transfer_i_to_N (i);
state[i] = false;
}
else if (hi[i]==0 && w[i] <= 0) {
lcp.transfer_i_to_N (i);
state[i] = true;
}
else if (w[i]==0) {
// this is a degenerate case. by the time we get to this test we know
// that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
// and similarly that hi > 0. this means that the line segment
// corresponding to set C is at least finite in extent, and we are on it.
// NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
lcp.solve1 (delta_x,i,0,1);
lcp.transfer_i_to_C (i);
}
else {
// we must push x(i) and w(i)
for (;;) {
int dir;
dReal dirf;
// find direction to push on x(i)
if (w[i] <= 0) {
dir = 1;
dirf = REAL(1.0);
}
else {
dir = -1;
dirf = REAL(-1.0);
}
// compute: delta_x(C) = -dir*A(C,C)\A(C,i)
lcp.solve1 (delta_x,i,dir);
// note that delta_x[i] = dirf, but we wont bother to set it
// compute: delta_w = A*delta_x ... note we only care about
// delta_w(N) and delta_w(i), the rest is ignored
lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
lcp.pN_plusequals_ANi (delta_w,i,dir);
delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i)*dirf;
// find largest step we can take (size=s), either to drive x(i),w(i)
// to the valid LCP region or to drive an already-valid variable
// outside the valid region.
int cmd = 1; // index switching command
int si = 0; // si = index to switch if cmd>3
dReal s = -w[i]/delta_w[i];
if (dir > 0) {
if (hi[i] < dInfinity) {
dReal s2 = (hi[i]-x[i])*dirf; // was (hi[i]-x[i])/dirf // step to x(i)=hi(i)
if (s2 < s) {
s = s2;
cmd = 3;
}
}
}
else {
if (lo[i] > -dInfinity) {
dReal s2 = (lo[i]-x[i])*dirf; // was (lo[i]-x[i])/dirf // step to x(i)=lo(i)
if (s2 < s) {
s = s2;
cmd = 2;
}
}
}
{
const int numN = lcp.numN();
for (int k=0; k < numN; ++k) {
const int indexN_k = lcp.indexN(k);
if (!state[indexN_k] ? delta_w[indexN_k] < 0 : delta_w[indexN_k] > 0) {
// don't bother checking if lo=hi=0
if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue;
dReal s2 = -w[indexN_k] / delta_w[indexN_k];
if (s2 < s) {
s = s2;
cmd = 4;
si = indexN_k;
}
}
}
}
{
const int numC = lcp.numC();
for (int k=adj_nub; k < numC; ++k) {
const int indexC_k = lcp.indexC(k);
if (delta_x[indexC_k] < 0 && lo[indexC_k] > -dInfinity) {
dReal s2 = (lo[indexC_k]-x[indexC_k]) / delta_x[indexC_k];
if (s2 < s) {
s = s2;
cmd = 5;
si = indexC_k;
}
}
if (delta_x[indexC_k] > 0 && hi[indexC_k] < dInfinity) {
dReal s2 = (hi[indexC_k]-x[indexC_k]) / delta_x[indexC_k];
if (s2 < s) {
s = s2;
cmd = 6;
si = indexC_k;
}
}
}
}
//static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
// "C->NL","C->NH"};
//printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
// if s <= 0 then we've got a problem. if we just keep going then
// we're going to get stuck in an infinite loop. instead, just cross
// our fingers and exit with the current solution.
if (s <= REAL(0.0)) {
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",(double)s);
if (i < n) {
dSetZero (x+i,n-i);
dSetZero (w+i,n-i);
}
s_error = true;
break;
}
// apply x = x + s * delta_x
lcp.pC_plusequals_s_times_qC (x, s, delta_x);
x[i] += s * dirf;
// apply w = w + s * delta_w
lcp.pN_plusequals_s_times_qN (w, s, delta_w);
w[i] += s * delta_w[i];
void *tmpbuf;
// switch indexes between sets if necessary
switch (cmd) {
case 1: // done
w[i] = 0;
lcp.transfer_i_to_C (i);
break;
case 2: // done
x[i] = lo[i];
state[i] = false;
lcp.transfer_i_to_N (i);
break;
case 3: // done
x[i] = hi[i];
state[i] = true;
lcp.transfer_i_to_N (i);
break;
case 4: // keep going
w[si] = 0;
lcp.transfer_i_from_N_to_C (si);
break;
case 5: // keep going
x[si] = lo[si];
state[si] = false;
tmpbuf = memarena->PeekBufferRemainder();
lcp.transfer_i_from_C_to_N (si, tmpbuf);
break;
case 6: // keep going
x[si] = hi[si];
state[si] = true;
tmpbuf = memarena->PeekBufferRemainder();
lcp.transfer_i_from_C_to_N (si, tmpbuf);
break;
}
if (cmd <= 3) break;
} // for (;;)
} // else
if (s_error) {
break;
}
} // for (int i=adj_nub; i<n; ++i)
lcp.unpermute();
}
static size_t BuildIslandsAndEstimateStepperMemoryRequirements(dxWorldProcessContext *context,
dxWorld *world, dReal stepsize, dmemestimate_fn_t stepperestimate)
{
const int sizeelements = 2;
size_t maxreq = 0;
// handle auto-disabling of bodies
dInternalHandleAutoDisabling (world,stepsize);
int nb = world->nb, nj = world->nj;
// Make array for island body/joint counts
int *islandsizes = context->AllocateArray<int>(2 * nb);
int *sizescurr;
// make arrays for body and joint lists (for a single island) to go into
dxBody **body = context->AllocateArray<dxBody *>(nb);
dxJoint **joint = context->AllocateArray<dxJoint *>(nj);
BEGIN_STATE_SAVE(context, stackstate) {
// allocate a stack of unvisited bodies in the island. the maximum size of
// the stack can be the lesser of the number of bodies or joints, because
// new bodies are only ever added to the stack by going through untagged
// joints. all the bodies in the stack must be tagged!
int stackalloc = (nj < nb) ? nj : nb;
dxBody **stack = context->AllocateArray<dxBody *>(stackalloc);
{
// set all body/joint tags to 0
for (dxBody *b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0;
for (dxJoint *j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0;
}
sizescurr = islandsizes;
dxBody **bodystart = body;
dxJoint **jointstart = joint;
for (dxBody *bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
// get bb = the next enabled, untagged body, and tag it
if (!bb->tag) {
if (!(bb->flags & dxBodyDisabled)) {
bb->tag = 1;
dxBody **bodycurr = bodystart;
dxJoint **jointcurr = jointstart;
// tag all bodies and joints starting from bb.
*bodycurr++ = bb;
int stacksize = 0;
dxBody *b = bb;
while (true) {
// traverse and tag all body's joints, add untagged connected bodies
// to stack
for (dxJointNode *n=b->firstjoint; n; n=n->next) {
dxJoint *njoint = n->joint;
if (!njoint->tag) {
if (njoint->isEnabled()) {
njoint->tag = 1;
*jointcurr++ = njoint;
dxBody *nbody = n->body;
// Body disabled flag is not checked here. This is how auto-enable works.
if (nbody && nbody->tag <= 0) {
nbody->tag = 1;
// Make sure all bodies are in the enabled state.
nbody->flags &= ~dxBodyDisabled;
stack[stacksize++] = nbody;
}
} else {
njoint->tag = -1; // Used in Step to prevent search over disabled joints (not needed for QuickStep so far)
}
}
}
dIASSERT(stacksize <= world->nb);
dIASSERT(stacksize <= world->nj);
if (stacksize == 0) {
break;
}
b = stack[--stacksize]; // pop body off stack
*bodycurr++ = b; // put body on body list
}
int bcount = bodycurr - bodystart;
int jcount = jointcurr - jointstart;
sizescurr[0] = bcount;
sizescurr[1] = jcount;
sizescurr += sizeelements;
size_t islandreq = stepperestimate(bodystart, bcount, jointstart, jcount);
maxreq = (maxreq > islandreq) ? maxreq : islandreq;
bodystart = bodycurr;
jointstart = jointcurr;
} else {
bb->tag = -1; // Not used so far (assigned to retain consistency with joints)
}
}
}
} END_STATE_SAVE(context, stackstate);
void sCylinderBoxData::_cldClipBoxToCylinder()
{
dIASSERT(m_nContacts != (m_iFlags & NUMC_MASK));
dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
// check which circle from cylinder we take for clipping
if ( dVector3Dot(m_vCylinderAxis, m_vNormal) > REAL(0.0) )
{
// get top circle
vCylinderCirclePos[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5));
vCylinderCirclePos[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5));
vCylinderCirclePos[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5));
vCylinderCircleNormal_Rel[0] = REAL(0.0);
vCylinderCircleNormal_Rel[1] = REAL(0.0);
vCylinderCircleNormal_Rel[2] = REAL(0.0);
vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0);
}
else
{
// get bottom circle
vCylinderCirclePos[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5));
vCylinderCirclePos[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5));
vCylinderCirclePos[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5));
vCylinderCircleNormal_Rel[0] = REAL(0.0);
vCylinderCircleNormal_Rel[1] = REAL(0.0);
vCylinderCircleNormal_Rel[2] = REAL(0.0);
vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
}
// vNr is normal in Box frame, pointing from Cylinder to Box
dVector3 vNr;
dMatrix3 mBoxInv;
// Find a way to use quaternion
dMatrix3Inv(m_mBoxRot,mBoxInv);
dMultiplyMat3Vec3(mBoxInv,m_vNormal,vNr);
dVector3 vAbsNormal;
vAbsNormal[0] = dFabs( vNr[0] );
vAbsNormal[1] = dFabs( vNr[1] );
vAbsNormal[2] = dFabs( vNr[2] );
// find which face in box is closest to cylinder
int iB0, iB1, iB2;
// Different from Croteam's code
if (vAbsNormal[1] > vAbsNormal[0])
{
// 1 > 0
if (vAbsNormal[0]> vAbsNormal[2])
{
// 0 > 2 -> 1 > 0 >2
iB0 = 1; iB1 = 0; iB2 = 2;
}
else
{
// 2 > 0-> Must compare 1 and 2
if (vAbsNormal[1] > vAbsNormal[2])
{
// 1 > 2 -> 1 > 2 > 0
iB0 = 1; iB1 = 2; iB2 = 0;
}
else
{
// 2 > 1 -> 2 > 1 > 0;
iB0 = 2; iB1 = 1; iB2 = 0;
}
}
}
else
{
// 0 > 1
if (vAbsNormal[1] > vAbsNormal[2])
{
// 1 > 2 -> 0 > 1 > 2
iB0 = 0; iB1 = 1; iB2 = 2;
}
else
{
// 2 > 1 -> Must compare 0 and 2
if (vAbsNormal[0] > vAbsNormal[2])
{
// 0 > 2 -> 0 > 2 > 1;
iB0 = 0; iB1 = 2; iB2 = 1;
}
else
{
// 2 > 0 -> 2 > 0 > 1;
iB0 = 2; iB1 = 0; iB2 = 1;
}
}
}
dVector3 vCenter;
// find center of box polygon
dVector3 vTemp;
if (vNr[iB0] > 0)
{
dMat3GetCol(m_mBoxRot,iB0,vTemp);
vCenter[0] = m_vBoxPos[0] - m_vBoxHalfSize[iB0]*vTemp[0];
vCenter[1] = m_vBoxPos[1] - m_vBoxHalfSize[iB0]*vTemp[1];
vCenter[2] = m_vBoxPos[2] - m_vBoxHalfSize[iB0]*vTemp[2];
}
else
{
dMat3GetCol(m_mBoxRot,iB0,vTemp);
vCenter[0] = m_vBoxPos[0] + m_vBoxHalfSize[iB0]*vTemp[0];
vCenter[1] = m_vBoxPos[1] + m_vBoxHalfSize[iB0]*vTemp[1];
vCenter[2] = m_vBoxPos[2] + m_vBoxHalfSize[iB0]*vTemp[2];
}
// find the vertices of box polygon
dVector3 avPoints[4];
dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS];
dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS];
int i=0;
for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++)
{
avTempArray1[i][0] = REAL(0.0);
avTempArray1[i][1] = REAL(0.0);
avTempArray1[i][2] = REAL(0.0);
avTempArray2[i][0] = REAL(0.0);
avTempArray2[i][1] = REAL(0.0);
avTempArray2[i][2] = REAL(0.0);
}
dVector3 vAxis1, vAxis2;
dMat3GetCol(m_mBoxRot,iB1,vAxis1);
dMat3GetCol(m_mBoxRot,iB2,vAxis2);
avPoints[0][0] = vCenter[0] + m_vBoxHalfSize[iB1] * vAxis1[0] - m_vBoxHalfSize[iB2] * vAxis2[0];
avPoints[0][1] = vCenter[1] + m_vBoxHalfSize[iB1] * vAxis1[1] - m_vBoxHalfSize[iB2] * vAxis2[1];
avPoints[0][2] = vCenter[2] + m_vBoxHalfSize[iB1] * vAxis1[2] - m_vBoxHalfSize[iB2] * vAxis2[2];
avPoints[1][0] = vCenter[0] - m_vBoxHalfSize[iB1] * vAxis1[0] - m_vBoxHalfSize[iB2] * vAxis2[0];
avPoints[1][1] = vCenter[1] - m_vBoxHalfSize[iB1] * vAxis1[1] - m_vBoxHalfSize[iB2] * vAxis2[1];
avPoints[1][2] = vCenter[2] - m_vBoxHalfSize[iB1] * vAxis1[2] - m_vBoxHalfSize[iB2] * vAxis2[2];
avPoints[2][0] = vCenter[0] - m_vBoxHalfSize[iB1] * vAxis1[0] + m_vBoxHalfSize[iB2] * vAxis2[0];
avPoints[2][1] = vCenter[1] - m_vBoxHalfSize[iB1] * vAxis1[1] + m_vBoxHalfSize[iB2] * vAxis2[1];
avPoints[2][2] = vCenter[2] - m_vBoxHalfSize[iB1] * vAxis1[2] + m_vBoxHalfSize[iB2] * vAxis2[2];
avPoints[3][0] = vCenter[0] + m_vBoxHalfSize[iB1] * vAxis1[0] + m_vBoxHalfSize[iB2] * vAxis2[0];
avPoints[3][1] = vCenter[1] + m_vBoxHalfSize[iB1] * vAxis1[1] + m_vBoxHalfSize[iB2] * vAxis2[1];
avPoints[3][2] = vCenter[2] + m_vBoxHalfSize[iB1] * vAxis1[2] + m_vBoxHalfSize[iB2] * vAxis2[2];
// transform box points to space of cylinder circle
dMatrix3 mCylinderInv;
dMatrix3Inv(m_mCylinderRot,mCylinderInv);
for(i=0; i<4; i++)
{
dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp);
dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]);
}
int iTmpCounter1 = 0;
int iTmpCounter2 = 0;
dVector4 plPlane;
// plane of cylinder that contains circle for intersection
dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane);
dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane);
// Body of base circle of Cylinder
int nCircleSegment = 0;
for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; nCircleSegment++)
{
dConstructPlane(m_avCylinderNormals[nCircleSegment],m_fCylinderRadius,plPlane);
if (0 == (nCircleSegment % 2))
{
dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane);
}
else
{
dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane );
}
dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= MAX_CYLBOX_CLIP_POINTS );
dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS );
}
// back transform clipped points to absolute space
dReal ftmpdot;
dReal fTempDepth;
dVector3 vPoint;
if (nCircleSegment % 2)
{
for( i=0; i<iTmpCounter2; i++)
{
dMultiply0_331(vPoint,m_mCylinderRot,avTempArray2[i]);
vPoint[0] += vCylinderCirclePos[0];
vPoint[1] += vCylinderCirclePos[1];
vPoint[2] += vCylinderCirclePos[2];
dVector3Subtract(vPoint,m_vCylinderPos,vTemp);
ftmpdot = dVector3Dot(vTemp, m_vNormal);
fTempDepth = m_fBestrc - ftmpdot;
// Depth must be positive
if (fTempDepth > REAL(0.0))
{
// generate contacts
dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip);
Contact0->depth = fTempDepth;
dVector3Copy(m_vNormal,Contact0->normal);
dVector3Copy(vPoint,Contact0->pos);
Contact0->g1 = m_gCylinder;
Contact0->g2 = m_gBox;
Contact0->side1 = -1;
Contact0->side2 = -1;
dVector3Inv(Contact0->normal);
m_nContacts++;
if (m_nContacts == (m_iFlags & NUMC_MASK))
{
break;
}
}
}
}
else
{
for( i=0; i<iTmpCounter1; i++)
{
dMultiply0_331(vPoint,m_mCylinderRot,avTempArray1[i]);
vPoint[0] += vCylinderCirclePos[0];
vPoint[1] += vCylinderCirclePos[1];
vPoint[2] += vCylinderCirclePos[2];
dVector3Subtract(vPoint,m_vCylinderPos,vTemp);
ftmpdot = dVector3Dot(vTemp, m_vNormal);
fTempDepth = m_fBestrc - ftmpdot;
// Depth must be positive
if (fTempDepth > REAL(0.0))
{
// generate contacts
dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip);
Contact0->depth = fTempDepth;
dVector3Copy(m_vNormal,Contact0->normal);
dVector3Copy(vPoint,Contact0->pos);
Contact0->g1 = m_gCylinder;
Contact0->g2 = m_gBox;
Contact0->side1 = -1;
Contact0->side2 = -1;
dVector3Inv(Contact0->normal);
m_nContacts++;
if (m_nContacts == (m_iFlags & NUMC_MASK))
{
break;
}
}
}
}
}
int sCylinderBoxData::_cldClipCylinderToBox()
{
dIASSERT(m_nContacts != (m_iFlags & NUMC_MASK));
// calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal
dVector3 vN;
dReal fTemp1 = dVector3Dot(m_vCylinderAxis,m_vNormal);
vN[0] = m_vNormal[0] - m_vCylinderAxis[0]*fTemp1;
vN[1] = m_vNormal[1] - m_vCylinderAxis[1]*fTemp1;
vN[2] = m_vNormal[2] - m_vCylinderAxis[2]*fTemp1;
// normalize that vector
dNormalize3(vN);
// translate cylinder end points by the vector
dVector3 vCposTrans;
vCposTrans[0] = m_vCylinderPos[0] + vN[0] * m_fCylinderRadius;
vCposTrans[1] = m_vCylinderPos[1] + vN[1] * m_fCylinderRadius;
vCposTrans[2] = m_vCylinderPos[2] + vN[2] * m_fCylinderRadius;
m_vEp0[0] = vCposTrans[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5));
m_vEp0[1] = vCposTrans[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5));
m_vEp0[2] = vCposTrans[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5));
m_vEp1[0] = vCposTrans[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5));
m_vEp1[1] = vCposTrans[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5));
m_vEp1[2] = vCposTrans[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5));
// transform edge points in box space
m_vEp0[0] -= m_vBoxPos[0];
m_vEp0[1] -= m_vBoxPos[1];
m_vEp0[2] -= m_vBoxPos[2];
m_vEp1[0] -= m_vBoxPos[0];
m_vEp1[1] -= m_vBoxPos[1];
m_vEp1[2] -= m_vBoxPos[2];
dVector3 vTemp1;
// clip the edge to box
dVector4 plPlane;
// plane 0 +x
dMat3GetCol(m_mBoxRot,0,vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[0],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// plane 1 +y
dMat3GetCol(m_mBoxRot,1,vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[1],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// plane 2 +z
dMat3GetCol(m_mBoxRot,2,vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[2],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// plane 3 -x
dMat3GetCol(m_mBoxRot,0,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[0],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// plane 4 -y
dMat3GetCol(m_mBoxRot,1,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[1],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// plane 5 -z
dMat3GetCol(m_mBoxRot,2,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,m_vBoxHalfSize[2],plPlane);
if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane ))
{
return 0;
}
// calculate depths for both contact points
m_fDepth0 = m_fBestrb + dVector3Dot(m_vEp0, m_vNormal);
m_fDepth1 = m_fBestrb + dVector3Dot(m_vEp1, m_vNormal);
// clamp depths to 0
if(m_fDepth0<0)
{
m_fDepth0 = REAL(0.0);
}
if(m_fDepth1<0)
{
m_fDepth1 = REAL(0.0);
}
// back transform edge points from box to absolute space
m_vEp0[0] += m_vBoxPos[0];
m_vEp0[1] += m_vBoxPos[1];
m_vEp0[2] += m_vBoxPos[2];
m_vEp1[0] += m_vBoxPos[0];
m_vEp1[1] += m_vBoxPos[1];
m_vEp1[2] += m_vBoxPos[2];
dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip);
Contact0->depth = m_fDepth0;
dVector3Copy(m_vNormal,Contact0->normal);
dVector3Copy(m_vEp0,Contact0->pos);
Contact0->g1 = m_gCylinder;
Contact0->g2 = m_gBox;
Contact0->side1 = -1;
Contact0->side2 = -1;
dVector3Inv(Contact0->normal);
m_nContacts++;
if (m_nContacts != (m_iFlags & NUMC_MASK))
{
dContactGeom* Contact1 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip);
Contact1->depth = m_fDepth1;
dVector3Copy(m_vNormal,Contact1->normal);
dVector3Copy(m_vEp1,Contact1->pos);
Contact1->g1 = m_gCylinder;
Contact1->g2 = m_gBox;
Contact1->side1 = -1;
Contact1->side2 = -1;
dVector3Inv(Contact1->normal);
m_nContacts++;
}
return 1;
}
// sorts out islands,
// cllocates array for island information into arrays: body[nj], joint[nb], islandsizes[2*nb]
// context->SavePreallocations(islandcount, islandsizes, body, joint,islandreqs);
// and put into context
//
static size_t BuildIslandsAndEstimateStepperMemoryRequirements(dxWorldProcessContext *context,
dxWorld *world, dReal stepsize, dmemestimate_fn_t stepperestimate)
{
const int sizeelements = 2;
size_t maxreq = 0;
// handle auto-disabling of bodies
dInternalHandleAutoDisabling (world,stepsize);
int nb = world->nb, nj = world->nj;
// Make array for island body/joint counts
int *islandsizes = context->AllocateArray<int>(2 * nb);
int *sizescurr;
// an array to save all islandreqs for each thread
size_t *islandreqs = context->AllocateArray<size_t>(nb);
size_t *islandreqscurr;
// make arrays for body and joint lists (for a single island) to go into
dxBody **body = context->AllocateArray<dxBody *>(nb); // allocates a block of pointers and get back a pointer to first element
dxJoint **joint = context->AllocateArray<dxJoint *>(nj); // allocates a block of pointers and get back a pointer to first element
BEGIN_STATE_SAVE(context, stackstate) {
// stack is used to hold untagged bodies when traversing through all the joint-linked bodies.
// at the end, all the bodies in the stack are popped back out into the island.
//
// allocate a stack of UNVISITED BODIES in the island. the maximum size of
// the stack can be the lesser of the number of bodies or joints, because
// new bodies are only ever added to the stack by going through untagged
// joints. all the bodies in the stack must be tagged!
int stackalloc = (nj < nb) ? nj : nb;
dxBody **stack = context->AllocateArray<dxBody *>(stackalloc); // a body stack
{
// set all body/joint island_tags to 0
for (dxBody *b=world->firstbody; b; b=(dxBody*)b->next) b->island_tag = 0;
for (dxJoint *j=world->firstjoint; j; j=(dxJoint*)j->next) j->island_tag = 0;
}
//int island_count = 0;
sizescurr = islandsizes;
islandreqscurr = islandreqs;
dxBody **bodystart = body;
dxJoint **jointstart = joint;
// loop through all body, tag each one as it is processed
// every step in this for loop is one island
for (dxBody *bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
// get bb = the next enabled, untagged body, and tag it
if (!bb->island_tag) {
if (!(bb->flags & dxBodyDisabled)) {
bb->island_tag = 1;
dxBody **bodycurr = bodystart;
dxJoint **jointcurr = jointstart;
// tag all bodies and joints starting from bb.
*bodycurr++ = bb;
int stacksize = 0;
dxBody *b = bb;
while (true) {
// traverse and island_tag all body's joints, add untagged connected bodies
// to stack
for (dxJointNode *n=b->firstjoint; n; n=n->next) {
dxJoint *njoint = n->joint;
if (!njoint->island_tag) {
if (njoint->isEnabled()) {
njoint->island_tag = 1;
*jointcurr++ = njoint;
dxBody *nbody = n->body;
// Body disabled flag is not checked here. This is how auto-enable works.
if (nbody && nbody->island_tag <= 0) {
nbody->island_tag = 1;
// Make sure all bodies are in the enabled state.
nbody->flags &= ~dxBodyDisabled;
stack[stacksize++] = nbody;
}
} else {
njoint->tag = -1; // Used in Step to prevent search over disabled joints (not needed for QuickStep so far)
}
}
}
dIASSERT(stacksize <= world->nb);
dIASSERT(stacksize <= world->nj);
if (stacksize == 0) {
break;
}
b = stack[--stacksize]; // pop body off stack
*bodycurr++ = b; // put body on body list
}
int bcount = bodycurr - bodystart;
int jcount = jointcurr - jointstart;
sizescurr[0] = bcount;
sizescurr[1] = jcount;
sizescurr += sizeelements;
// save individual islandreq for each island separately
*islandreqscurr = stepperestimate(bodystart, bcount, jointstart, jcount);
maxreq = (maxreq > *islandreqscurr) ? maxreq : *islandreqscurr;
//printf("island %d complete, stepper %d maxreq %d \n",island_count++,*islandreqscurr, maxreq);
islandreqscurr += 1;
bodystart = bodycurr;
jointstart = jointcurr;
} else {
bb->island_tag = -1; // Not used so far (assigned to retain consistency with joints)
}
}
}
} END_STATE_SAVE(context, stackstate); // restores contex pointer m_pAllocCurrent back to what it was before this block
int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere,
int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (Cylinder->type == dCylinderClass);
dIASSERT (Sphere->type == dSphereClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
//unsigned char* pContactData = (unsigned char*)contact;
int GeomCount = 0; // count of used contacts
#ifdef dSINGLE
const dReal toleranz = REAL(0.0001);
#endif
#ifdef dDOUBLE
const dReal toleranz = REAL(0.0000001);
#endif
// get the data from the geoms
dReal radius, length;
dGeomCylinderGetParams(Cylinder, &radius, &length);
dVector3 &cylpos = Cylinder->final_posr->pos;
//const dReal* pfRot1 = dGeomGetRotation(Cylinder);
dReal radius2;
radius2 = dGeomSphereGetRadius(Sphere);
const dReal* SpherePos = dGeomGetPosition(Sphere);
// G1Pos1 is the middle of the first disc
// G1Pos2 is the middle of the second disc
// vDir1 is the unit direction of the cylinderaxis
dVector3 G1Pos1, G1Pos2, vDir1;
vDir1[0] = Cylinder->final_posr->R[2];
vDir1[1] = Cylinder->final_posr->R[6];
vDir1[2] = Cylinder->final_posr->R[10];
dReal s;
s = length * REAL(0.5); // just a precomputed factor
G1Pos2[0] = vDir1[0] * s + cylpos[0];
G1Pos2[1] = vDir1[1] * s + cylpos[1];
G1Pos2[2] = vDir1[2] * s + cylpos[2];
G1Pos1[0] = vDir1[0] * -s + cylpos[0];
G1Pos1[1] = vDir1[1] * -s + cylpos[1];
G1Pos1[2] = vDir1[2] * -s + cylpos[2];
dVector3 C;
dReal t;
// Step 1: compute the two distances 's' and 't'
// 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle
s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2];
if(s < (-radius2) || s > (length + radius2) )
{
// Sphere is too far away from the discs
// no collision
return 0;
}
// C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis
C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0];
C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1];
C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2];
// t is the distance from the Sphere-middle to the cylinder-axis!
t = dCalcVectorLength3(C);
if(t > (radius + radius2) )
{
// Sphere is too far away from the cylinder axis!
// no collision
return 0;
}
// decide which kind of collision we have:
if(t > radius && (s < 0 || s > length) )
{
// 3. collision
if(s <= 0)
{
contact->depth = radius2 - dSqrt( (s) * (s) + (t - radius) * (t - radius) );
if(contact->depth < 0)
{
// no collision!
return 0;
}
contact->pos[0] = C[0] / t * -radius + G1Pos1[0];
contact->pos[1] = C[1] / t * -radius + G1Pos1[1];
contact->pos[2] = C[2] / t * -radius + G1Pos1[2];
contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
else
{
// now s is bigger than length here!
contact->depth = radius2 - dSqrt( (s - length) * (s - length) + (t - radius) * (t - radius) );
if(contact->depth < 0)
{
// no collision!
return 0;
}
contact->pos[0] = C[0] / t * -radius + G1Pos2[0];
contact->pos[1] = C[1] / t * -radius + G1Pos2[1];
contact->pos[2] = C[2] / t * -radius + G1Pos2[2];
contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
else if( (radius - t) <= s && (radius - t) <= (length - s) )
{
// 1. collsision
if(t > (radius2 + toleranz))
{
// cylinder-axis is outside the sphere
contact->depth = (radius2 + radius) - t;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
else
{
C[0] /= t;
C[1] /= t;
C[2] /= t;
contact->pos[0] = C[0] * radius2 + SpherePos[0];
contact->pos[1] = C[1] * radius2 + SpherePos[1];
contact->pos[2] = C[2] * radius2 + SpherePos[2];
contact->normal[0] = C[0];
contact->normal[1] = C[1];
contact->normal[2] = C[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
else
{
// cylinder-axis is outside of the sphere
contact->depth = (radius2 + radius) - t;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
else
{
contact->pos[0] = C[0] + SpherePos[0];
contact->pos[1] = C[1] + SpherePos[1];
contact->pos[2] = C[2] + SpherePos[2];
contact->normal[0] = C[0] / t;
contact->normal[1] = C[1] / t;
contact->normal[2] = C[2] / t;
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
}
else
{
// 2. collision
if(s <= (length * REAL(0.5)) )
{
// collsision with the first disc
contact->depth = s + radius2;
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
contact->pos[0] = radius2 * vDir1[0] + SpherePos[0];
contact->pos[1] = radius2 * vDir1[1] + SpherePos[1];
contact->pos[2] = radius2 * vDir1[2] + SpherePos[2];
contact->normal[0] = vDir1[0];
contact->normal[1] = vDir1[1];
contact->normal[2] = vDir1[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
else
{
// collsision with the second disc
contact->depth = (radius2 + length - s);
if(contact->depth < 0)
{
// should never happen, but just for safeness
return 0;
}
contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0];
contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1];
contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2];
contact->normal[0] = -vDir1[0];
contact->normal[1] = -vDir1[1];
contact->normal[2] = -vDir1[2];
contact->g1 = Cylinder;
contact->g2 = Sphere;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
return GeomCount;
}
}
return GeomCount;
}
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 = dDOT41(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 = dDOT(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 = dDOT44(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 = dDOT(r,r);
dReal B = 2*dDOT(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 = dDOT14(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);
}
int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (Cylinder->type == dCylinderClass);
dIASSERT (Plane->type == dPlaneClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
int GeomCount = 0; // count of used contactgeoms
#ifdef dSINGLE
const dReal toleranz = REAL(0.0001);
#endif
#ifdef dDOUBLE
const dReal toleranz = REAL(0.0000001);
#endif
// Get the properties of the cylinder (length+radius)
dReal radius, length;
dGeomCylinderGetParams(Cylinder, &radius, &length);
dVector3 &cylpos = Cylinder->final_posr->pos;
// and the plane
dVector4 planevec;
dGeomPlaneGetParams(Plane, planevec);
dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]};
//dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]};
dVector3 G1Pos1, G1Pos2, vDir1;
vDir1[0] = Cylinder->final_posr->R[2];
vDir1[1] = Cylinder->final_posr->R[6];
vDir1[2] = Cylinder->final_posr->R[10];
dReal s;
s = length * REAL(0.5);
G1Pos2[0] = vDir1[0] * s + cylpos[0];
G1Pos2[1] = vDir1[1] * s + cylpos[1];
G1Pos2[2] = vDir1[2] * s + cylpos[2];
G1Pos1[0] = vDir1[0] * -s + cylpos[0];
G1Pos1[1] = vDir1[1] * -s + cylpos[1];
G1Pos1[2] = vDir1[2] * -s + cylpos[2];
dVector3 C;
// parallel-check
s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2];
if(s < 0)
s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
else
s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
if(s < toleranz && s > (-toleranz))
{
// discs are parallel to the plane
// 1.compute if, and where contacts are
dVector3 P;
s = planevec[3] - dVector3Dot(planevec, G1Pos1);
dReal t;
t = planevec[3] - dVector3Dot(planevec, G1Pos2);
if(s >= t) // s == t does never happen,
{
if(s >= 0)
{
// 1. Disc
dVector3Copy(G1Pos1, P);
}
else
return GeomCount; // no contacts
}
else
{
if(t >= 0)
{
// 2. Disc
dVector3Copy(G1Pos2, P);
}
else
return GeomCount; // no contacts
}
// 2. generate a coordinate-system on the disc
dVector3 V1, V2;
if(vDir1[0] < toleranz && vDir1[0] > (-toleranz))
{
// not x-axis
V1[0] = vDir1[0] + REAL(1.0); // random value
V1[1] = vDir1[1];
V1[2] = vDir1[2];
}
else
{
// maybe x-axis
V1[0] = vDir1[0];
V1[1] = vDir1[1] + REAL(1.0); // random value
V1[2] = vDir1[2];
}
// V1 is now another direction than vDir1
// Cross-product
dVector3Cross(V1, vDir1, V2);
// make unit V2
t = dVector3Length(V2);
t = radius / t;
dVector3Scale(V2, t);
// cross again
dVector3Cross(V2, vDir1, V1);
// |V2| is 'radius' and vDir1 unit, so |V1| is 'radius'
// V1 = first axis
// V2 = second axis
// 3. generate contactpoints
// Potential contact 1
dVector3Add(P, V1, contact->pos);
contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
if(contact->depth > 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
// Potential contact 2
dVector3Subtract(P, V1, contact->pos);
contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
if(contact->depth > 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
// Potential contact 3
dVector3Add(P, V2, contact->pos);
contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
if(contact->depth > 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
// Potential contact 4
dVector3Subtract(P, V2, contact->pos);
contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
if(contact->depth > 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
}
else
{
dReal t = dVector3Dot(PlaneNormal, vDir1);
C[0] = vDir1[0] * t - PlaneNormal[0];
C[1] = vDir1[1] * t - PlaneNormal[1];
C[2] = vDir1[2] * t - PlaneNormal[2];
s = dVector3Length(C);
// move C onto the circle
s = radius / s;
dVector3Scale(C, s);
// deepest point of disc 1
dVector3Add(C, G1Pos1, contact->pos);
// depth of the deepest point
contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
if(contact->depth >= 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
// C is still computed
// deepest point of disc 2
dVector3Add(C, G1Pos2, contact->pos);
// depth of the deepest point
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth >= 0)
{
dVector3Copy(PlaneNormal, contact->normal);
contact->g1 = Cylinder;
contact->g2 = Plane;
contact->side1 = -1;
contact->side2 = -1;
GeomCount++;
if( GeomCount >= (flags & NUMC_MASK))
return GeomCount; // enough contactgeoms
contact = (dContactGeom *)((char *)contact + skip);
}
}
return GeomCount;
}
void _dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip, void *tmpbuf/*[2*nskip]*/)
{
dAASSERT (L && d && a && n > 0 && nskip >= n);
if (n < 2) return;
dReal *W1 = tmpbuf ? (dReal *)tmpbuf : (dReal*) ALLOCA ((2*nskip)*sizeof(dReal));
dReal *W2 = W1 + nskip;
W1[0] = REAL(0.0);
W2[0] = REAL(0.0);
for (int j=1; j<n; ++j) {
W1[j] = W2[j] = (dReal) (a[j] * M_SQRT1_2);
}
dReal W11 = (dReal) ((REAL(0.5)*a[0]+1)*M_SQRT1_2);
dReal W21 = (dReal) ((REAL(0.5)*a[0]-1)*M_SQRT1_2);
dReal alpha1 = REAL(1.0);
dReal alpha2 = REAL(1.0);
{
dReal dee = d[0];
dReal alphanew = alpha1 + (W11*W11)*dee;
dIASSERT(alphanew != dReal(0.0));
dee /= alphanew;
dReal gamma1 = W11 * dee;
dee *= alpha1;
alpha1 = alphanew;
alphanew = alpha2 - (W21*W21)*dee;
dee /= alphanew;
//dReal gamma2 = W21 * dee;
alpha2 = alphanew;
dReal k1 = REAL(1.0) - W21*gamma1;
dReal k2 = W21*gamma1*W11 - W21;
dReal *ll = L + nskip;
for (int p=1; p<n; ll+=nskip, ++p) {
dReal Wp = W1[p];
dReal ell = *ll;
W1[p] = Wp - W11*ell;
W2[p] = k1*Wp + k2*ell;
}
}
dReal *ll = L + (nskip + 1);
for (int j=1; j<n; ll+=nskip+1, ++j) {
dReal k1 = W1[j];
dReal k2 = W2[j];
dReal dee = d[j];
dReal alphanew = alpha1 + (k1*k1)*dee;
dIASSERT(alphanew != dReal(0.0));
dee /= alphanew;
dReal gamma1 = k1 * dee;
dee *= alpha1;
alpha1 = alphanew;
alphanew = alpha2 - (k2*k2)*dee;
dee /= alphanew;
dReal gamma2 = k2 * dee;
dee *= alpha2;
d[j] = dee;
alpha2 = alphanew;
dReal *l = ll + nskip;
for (int p=j+1; p<n; l+=nskip, ++p) {
dReal ell = *l;
dReal Wp = W1[p] - k1 * ell;
ell += gamma1 * Wp;
W1[p] = Wp;
Wp = W2[p] - k2 * ell;
ell -= gamma2 * Wp;
W2[p] = Wp;
*l = ell;
}
}
}
