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);
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache();
SphereCollider& Collider = pccColliderCache->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
Sphere.mCenter.x = Position[0];
Sphere.mCenter.y = Position[1];
Sphere.mCenter.z = Position[2];
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;
vu[0] = v1[0] - v0[0];
vu[1] = v1[1] - v0[1];
vu[2] = v1[2] - v0[2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = v2[0] - v0[0];
vv[1] = v2[1] - v0[1];
vv[2] = v2[2] - v0[2];
vv[3] = REAL(0.0);
// Get plane coefficients
dVector4 Plane;
dCROSS(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 = dDOT(Plane,Position) - dDOT(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;
ContactPos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
ContactPos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
ContactPos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
// Depth returned from GetContactData is depth along
// contact point - sphere center direction
// we'll project it to contact normal
dVector3 dir;
dir[0] = Position[0]-ContactPos[0];
dir[1] = Position[1]-ContactPos[1];
dir[2] = Position[2]-ContactPos[2];
dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, 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);
Contact->pos[0] = ContactPos[0];
Contact->pos[1] = ContactPos[1];
Contact->pos[2] = ContactPos[2];
Contact->pos[3] = REAL(0.0);
// Using normal as plane (reversed)
Contact->normal[0] = -Plane[0];
Contact->normal[1] = -Plane[1];
Contact->normal[2] = -Plane[2];
Contact->normal[3] = REAL(0.0);
Contact->depth = Depth * dirProj;
//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
#if !defined MERGECONTACTS // Merge all contacts into 1
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
#endif // Otherwise assigned later
Contact->side1 = TriIndex;
OutTriCount++;
}
#if defined MERGECONTACTS // Merge all contacts into 1
if (OutTriCount > 0){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT)){
dVector3 pos;
pos[0] = Contact->pos[0];
pos[1] = Contact->pos[1];
pos[2] = Contact->pos[2];
dVector3 normal;
normal[0] = Contact->normal[0] * Contact->depth;
normal[1] = Contact->normal[1] * Contact->depth;
normal[2] = Contact->normal[2] * Contact->depth;
int TriIndex = Contact->side1;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
pos[0] += TempContact->pos[0];
pos[1] += TempContact->pos[1];
pos[2] += TempContact->pos[2];
normal[0] += TempContact->normal[0] * TempContact->depth;
normal[1] += TempContact->normal[1] * TempContact->depth;
normal[2] += TempContact->normal[2] * TempContact->depth;
TriIndex = (TriMesh->TriMergeCallback) ? TriMesh->TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1;
}
Contact->side1 = TriIndex;
Contact->pos[0] = pos[0] / OutTriCount;
Contact->pos[1] = pos[1] / OutTriCount;
Contact->pos[2] = pos[2] / OutTriCount;
// Remember to divide in square space.
Contact->depth = dSqrt(dDOT(normal, normal) / OutTriCount);
if (Contact->depth > dEpsilon) { // otherwise the normal is too small
dVector3Copy(Contact->normal, normal);
dNormalize3(Contact->normal);
} // otherwise original Contact's normal would be used and it should be already normalized
}
return 1;
}
else return 0;
#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
if (OutTriCount != 0){
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
dVector3 Normal;
dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride);
Normal[0] = FirstContact->normal[0] * FirstContact->depth;
Normal[1] = FirstContact->normal[1] * FirstContact->depth;
Normal[2] = FirstContact->normal[2] * FirstContact->depth;
Normal[3] = FirstContact->normal[3] * FirstContact->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Normal[0] += Contact->normal[0] * Contact->depth;
Normal[1] += Contact->normal[1] * Contact->depth;
Normal[2] += Contact->normal[2] * Contact->depth;
Normal[3] += Contact->normal[3] * Contact->depth;
}
dNormalize3(Normal);
for (int i = 0; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->normal[0] = Normal[0];
Contact->normal[1] = Normal[1];
Contact->normal[2] = Normal[2];
Contact->normal[3] = Normal[3];
}
}
return OutTriCount;
}
else return 0;
#else // none of MERGECONTACTS and MERGECONTACTNORMALS // Just return
return OutTriCount;
#endif // MERGECONTACTS
}
else return 0;
// clip and generate contacts
static void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {
// if we have edge/edge intersection
if ( iBestAxis > 4 ) {
dVector3 vub,vPb,vPa;
SET(vPa,vHullBoxPos);
// calculate point on box edge
for( int i=0; i<3; i++) {
dVector3 vRotCol;
GETCOL(mHullBoxRot,i,vRotCol);
dReal fSign = dDOT(vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f;
vPa[0] += fSign * vBoxHalfSize[i] * vRotCol[0];
vPa[1] += fSign * vBoxHalfSize[i] * vRotCol[1];
vPa[2] += fSign * vBoxHalfSize[i] * vRotCol[2];
}
int iEdge = (iBestAxis-5)%3;
// decide which edge is on triangle
if ( iEdge == 0 ) {
SET(vPb,v0);
SET(vub,vE0);
} else if ( iEdge == 1) {
SET(vPb,v2);
SET(vub,vE1);
} else {
SET(vPb,v1);
SET(vub,vE2);
}
// setup direction parameter for face edge
dNormalize3(vub);
dReal fParam1, fParam2;
// setup direction parameter for box edge
dVector3 vua;
int col=(iBestAxis-5)/3;
GETCOL(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
#ifdef ORIG
if (ctContacts < (iFlags & 0x0ffff)) {
dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
Contact->depth = fBestDepth;
SET(Contact->normal,vBestNormal);
SET(Contact->pos,vPntTmp);
Contact->g1 = Geom1;
Contact->g2 = Geom2;
ctContacts++;
}
#endif
GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
vPntTmp, vBestNormal, fBestDepth, ctContacts);
// if triangle is the referent face then clip box to triangle face
} else if ( iBestAxis == 1 ) {
dVector3 vNormal2;
vNormal2[0]=-vBestNormal[0];
vNormal2[1]=-vBestNormal[1];
vNormal2[2]=-vBestNormal[2];
// vNr is normal in box frame, pointing from triangle to box
dMatrix3 mTransposed;
mTransposed[0*4+0]=mHullBoxRot[0*4+0];
mTransposed[0*4+1]=mHullBoxRot[1*4+0];
mTransposed[0*4+2]=mHullBoxRot[2*4+0];
mTransposed[1*4+0]=mHullBoxRot[0*4+1];
mTransposed[1*4+1]=mHullBoxRot[1*4+1];
mTransposed[1*4+2]=mHullBoxRot[2*4+1];
mTransposed[2*4+0]=mHullBoxRot[0*4+2];
mTransposed[2*4+1]=mHullBoxRot[1*4+2];
mTransposed[2*4+2]=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(mHullBoxRot,iB0,vRotCol);
if (vNr[iB0] > 0) {
vCenter[0] = vHullBoxPos[0] - v0[0] - vBoxHalfSize[iB0] * vRotCol[0];
vCenter[1] = vHullBoxPos[1] - v0[1] - vBoxHalfSize[iB0] * vRotCol[1];
vCenter[2] = vHullBoxPos[2] - v0[2] - vBoxHalfSize[iB0] * vRotCol[2];
} else {
vCenter[0] = vHullBoxPos[0] - v0[0] + vBoxHalfSize[iB0] * vRotCol[0];
vCenter[1] = vHullBoxPos[1] - v0[1] + vBoxHalfSize[iB0] * vRotCol[1];
vCenter[2] = vHullBoxPos[2] - v0[2] + vBoxHalfSize[iB0] * vRotCol[2];
}
// Here find 4 corner points of box
dVector3 avPoints[4];
dVector3 vRotCol2;
GETCOL(mHullBoxRot,iB1,vRotCol);
GETCOL(mHullBoxRot,iB2,vRotCol2);
for(int x=0;x<3;x++) {
avPoints[0][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[1][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[2][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
avPoints[3][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) + (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]=-vN[0];
vTemp[1]=-vN[1];
vTemp[2]=-vN[2];
dNormalize3(vTemp);
CONSTRUCTPLANE(plPlane,vTemp,0);
_cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane );
// Plane p0
dVector3 vTemp2;
SUBTRACT(v1,v0,vTemp2);
dCROSS(vTemp,=,vN,vTemp2);
dNormalize3(vTemp);
CONSTRUCTPLANE(plPlane,vTemp,0);
_cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane );
// Plane p1
SUBTRACT(v2,v1,vTemp2);
dCROSS(vTemp,=,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,=,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);
#ifdef ORIG
if (ctContacts < (iFlags & 0x0ffff)) {
dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
Contact->depth = -fTempDepth;
SET(Contact->normal,vBestNormal);
SET(Contact->pos,vPntTmp);
Contact->g1 = Geom1;
Contact->g2 = Geom2;
ctContacts++;
}
#endif
GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2,
vPntTmp, vBestNormal, -fTempDepth, ctContacts);
}
//dAASSERT(ctContacts>0);
// if box face is the referent face, then clip triangle on box face
} else { // 2 <= if iBestAxis <= 4
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 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);
SphereCollider& Collider = TriMesh->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
Sphere.mCenter.x = Position[0];
Sphere.mCenter.y = Position[1];
Sphere.mCenter.z = Position[2];
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(dxTriMesh::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;
vu[0] = v1[0] - v0[0];
vu[1] = v1[1] - v0[1];
vu[2] = v1[2] - v0[2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = v2[0] - v0[0];
vv[1] = v2[1] - v0[1];
vv[2] = v2[2] - v0[2];
vv[3] = REAL(0.0);
// Get plane coefficients
dVector4 Plane;
dCROSS(Plane, =, vu, vv);
dReal Area = dSqrt(dDOT(Plane, Plane)); // We can use this later
Plane[0] /= Area;
Plane[1] /= Area;
Plane[2] /= Area;
Plane[3] = dDOT(Plane, v0);
/* 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 = dDOT(Plane,Position) - Plane[3];
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)){
Depth = REAL(0.0);
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dReal w = REAL(1.0) - u - v;
Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
Contact->pos[3] = REAL(0.0);
// Using normal as plane (reversed)
Contact->normal[0] = -Plane[0];
Contact->normal[1] = -Plane[1];
Contact->normal[2] = -Plane[2];
Contact->normal[3] = REAL(0.0);
// Depth returned from GetContactData is depth along
// contact point - sphere center direction
// we'll project it to contact normal
dVector3 dir;
dir[0] = Position[0]-Contact->pos[0];
dir[1] = Position[1]-Contact->pos[1];
dir[2] = Position[2]-Contact->pos[2];
dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir));
Contact->depth = Depth * dirProj;
//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
Contact->side1 = TriIndex;
//Contact->g1 = TriMesh;
//Contact->g2 = SphereGeom;
OutTriCount++;
}
#ifdef MERGECONTACTS // Merge all contacts into 1
if (OutTriCount != 0){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
Contact->normal[0] *= Contact->depth;
Contact->normal[1] *= Contact->depth;
Contact->normal[2] *= Contact->depth;
Contact->normal[3] *= Contact->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->pos[0] += TempContact->pos[0];
Contact->pos[1] += TempContact->pos[1];
Contact->pos[2] += TempContact->pos[2];
Contact->pos[3] += TempContact->pos[3];
Contact->normal[0] += TempContact->normal[0] * TempContact->depth;
Contact->normal[1] += TempContact->normal[1] * TempContact->depth;
Contact->normal[2] += TempContact->normal[2] * TempContact->depth;
Contact->normal[3] += TempContact->normal[3] * TempContact->depth;
}
Contact->pos[0] /= OutTriCount;
Contact->pos[1] /= OutTriCount;
Contact->pos[2] /= OutTriCount;
Contact->pos[3] /= OutTriCount;
// Remember to divide in square space.
Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount);
dNormalize3(Contact->normal);
}
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
// TODO:
// Side1 now contains index of triangle that gave first hit
// Probably we should find index of triangle with deepest penetration
return 1;
}
else return 0;
#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
if (OutTriCount != 0){
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal;
Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Normal[0] += Contact->normal[0] * Contact->depth;
Normal[1] += Contact->normal[1] * Contact->depth;
Normal[2] += Contact->normal[2] * Contact->depth;
Normal[3] += Contact->normal[3] * Contact->depth;
}
dNormalize3(Normal);
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->normal[0] = Normal[0];
Contact->normal[1] = Normal[1];
Contact->normal[2] = Normal[2];
Contact->normal[3] = Normal[3];
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
}
else{
SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh;
SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom;
}
return OutTriCount;
}
else return 0;
#else //MERGECONTACTNORMALS // Just gather penetration depths and return
for (int i = 0; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
//Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal));
/*Contact->normal[0] /= Contact->depth;
Contact->normal[1] /= Contact->depth;
Contact->normal[2] /= Contact->depth;
Contact->normal[3] /= Contact->depth;*/
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
return OutTriCount;
#endif // MERGECONTACTS
}
else return 0;
int _cldClipCylinderToBox(sCylinderBoxData& cData)
{
// calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal
dVector3 vN;
dReal fTemp1 = dVector3Dot(cData.vCylinderAxis,cData.vNormal);
vN[0] = cData.vNormal[0] - cData.vCylinderAxis[0]*fTemp1;
vN[1] = cData.vNormal[1] - cData.vCylinderAxis[1]*fTemp1;
vN[2] = cData.vNormal[2] - cData.vCylinderAxis[2]*fTemp1;
// normalize that vector
dNormalize3(vN);
// translate cylinder end points by the vector
dVector3 vCposTrans;
vCposTrans[0] = cData.vCylinderPos[0] + vN[0] * cData.fCylinderRadius;
vCposTrans[1] = cData.vCylinderPos[1] + vN[1] * cData.fCylinderRadius;
vCposTrans[2] = cData.vCylinderPos[2] + vN[2] * cData.fCylinderRadius;
cData.vEp0[0] = vCposTrans[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
cData.vEp0[1] = vCposTrans[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
cData.vEp0[2] = vCposTrans[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
cData.vEp1[0] = vCposTrans[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
cData.vEp1[1] = vCposTrans[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
cData.vEp1[2] = vCposTrans[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5));
// transform edge points in box space
cData.vEp0[0] -= cData.vBoxPos[0];
cData.vEp0[1] -= cData.vBoxPos[1];
cData.vEp0[2] -= cData.vBoxPos[2];
cData.vEp1[0] -= cData.vBoxPos[0];
cData.vEp1[1] -= cData.vBoxPos[1];
cData.vEp1[2] -= cData.vBoxPos[2];
dVector3 vTemp1;
// clip the edge to box
dVector4 plPlane;
// plane 0 +x
dMat3GetCol(cData.mBoxRot,0,vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 1 +y
dMat3GetCol(cData.mBoxRot,1,vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 2 +z
dMat3GetCol(cData.mBoxRot,2,vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 3 -x
dMat3GetCol(cData.mBoxRot,0,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 4 -y
dMat3GetCol(cData.mBoxRot,1,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// plane 5 -z
dMat3GetCol(cData.mBoxRot,2,vTemp1);
dVector3Inv(vTemp1);
dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
{
return 0;
}
// calculate depths for both contact points
cData.fDepth0 = cData.fBestrb + dVector3Dot(cData.vEp0, cData.vNormal);
cData.fDepth1 = cData.fBestrb + dVector3Dot(cData.vEp1, cData.vNormal);
// clamp depths to 0
if(cData.fDepth0<0)
{
cData.fDepth0 = REAL(0.0);
}
if(cData.fDepth1<0)
{
cData.fDepth1 = REAL(0.0);
}
// back transform edge points from box to absolute space
cData.vEp0[0] += cData.vBoxPos[0];
cData.vEp0[1] += cData.vBoxPos[1];
cData.vEp0[2] += cData.vBoxPos[2];
cData.vEp1[0] += cData.vBoxPos[0];
cData.vEp1[1] += cData.vBoxPos[1];
cData.vEp1[2] += cData.vBoxPos[2];
dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact0->depth = cData.fDepth0;
dVector3Copy(cData.vNormal,Contact0->normal);
dVector3Copy(cData.vEp0,Contact0->pos);
Contact0->g1 = cData.gCylinder;
Contact0->g2 = cData.gBox;
dVector3Inv(Contact0->normal);
cData.nContacts++;
dContactGeom* Contact1 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact1->depth = cData.fDepth1;
dVector3Copy(cData.vNormal,Contact1->normal);
dVector3Copy(cData.vEp1,Contact1->pos);
Contact1->g1 = cData.gCylinder;
Contact1->g2 = cData.gBox;
dVector3Inv(Contact1->normal);
cData.nContacts++;
return 1;
}
void _cldClipBoxToCylinder(sCylinderBoxData& cData )
{
dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
// check which circle from cylinder we take for clipping
if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > REAL(0.0) )
{
// get top circle
vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.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] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5));
vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5));
vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.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(cData.mBoxRot,mBoxInv);
dMultiplyMat3Vec3(mBoxInv,cData.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(cData.mBoxRot,iB0,vTemp);
vCenter[0] = cData.vBoxPos[0] - cData.vBoxHalfSize[iB0]*vTemp[0];
vCenter[1] = cData.vBoxPos[1] - cData.vBoxHalfSize[iB0]*vTemp[1];
vCenter[2] = cData.vBoxPos[2] - cData.vBoxHalfSize[iB0]*vTemp[2];
}
else
{
dMat3GetCol(cData.mBoxRot,iB0,vTemp);
vCenter[0] = cData.vBoxPos[0] + cData.vBoxHalfSize[iB0]*vTemp[0];
vCenter[1] = cData.vBoxPos[1] + cData.vBoxHalfSize[iB0]*vTemp[1];
vCenter[2] = cData.vBoxPos[2] + cData.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(cData.mBoxRot,iB1,vAxis1);
dMat3GetCol(cData.mBoxRot,iB2,vAxis2);
avPoints[0][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[0][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[0][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];
avPoints[1][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[1][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[1][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2];
avPoints[2][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[2][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[2][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];
avPoints[3][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0];
avPoints[3][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1];
avPoints[3][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2];
// transform box points to space of cylinder circle
dMatrix3 mCylinderInv;
dMatrix3Inv(cData.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(cData.avCylinderNormals[nCircleSegment],cData.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,cData.mCylinderRot,avTempArray2[i]);
vPoint[0] += vCylinderCirclePos[0];
vPoint[1] += vCylinderCirclePos[1];
vPoint[2] += vCylinderCirclePos[2];
dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
ftmpdot = dVector3Dot(vTemp, cData.vNormal);
fTempDepth = cData.fBestrc - ftmpdot;
// Depth must be positive
if (fTempDepth > REAL(0.0))
{
// generate contacts
dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact0->depth = fTempDepth;
dVector3Copy(cData.vNormal,Contact0->normal);
dVector3Copy(vPoint,Contact0->pos);
Contact0->g1 = cData.gCylinder;
Contact0->g2 = cData.gBox;
dVector3Inv(Contact0->normal);
cData.nContacts++;
}
}
}
else
{
for( i=0; i<iTmpCounter1; i++)
{
dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray1[i]);
vPoint[0] += vCylinderCirclePos[0];
vPoint[1] += vCylinderCirclePos[1];
vPoint[2] += vCylinderCirclePos[2];
dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
ftmpdot = dVector3Dot(vTemp, cData.vNormal);
fTempDepth = cData.fBestrc - ftmpdot;
// Depth must be positive
if (fTempDepth > REAL(0.0))
{
// generate contacts
dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
Contact0->depth = fTempDepth;
dVector3Copy(cData.vNormal,Contact0->normal);
dVector3Copy(vPoint,Contact0->pos);
Contact0->g1 = cData.gCylinder;
Contact0->g2 = cData.gBox;
dVector3Inv(Contact0->normal);
cData.nContacts++;
}
}
}
}
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (RayGeom->type == dRayClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)g1;
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == RayGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
RayCollider& Collider = pccColliderCache->_RayCollider;
dReal Length = dGeomRayGetLength(RayGeom);
int FirstContact, BackfaceCull;
dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
int ClosestHit = dGeomRayGetClosestHit(RayGeom);
Collider.SetFirstContact(FirstContact != 0);
Collider.SetClosestHit(ClosestHit != 0);
Collider.SetCulling(BackfaceCull != 0);
Collider.SetMaxDist(Length);
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
/* Make Ray */
Ray WorldRay;
WorldRay.mOrig.x = Origin[0];
WorldRay.mOrig.y = Origin[1];
WorldRay.mOrig.z = Origin[2];
WorldRay.mDir.x = Direction[0];
WorldRay.mDir.y = Direction[1];
WorldRay.mDir.z = Direction[2];
/* Intersect */
Matrix4x4 amatrix;
int TriCount = 0;
if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
TriCount = pccColliderCache->Faces.GetNbFaces();
}
if (TriCount == 0) {
return 0;
}
const CollisionFace* Faces = pccColliderCache->Faces.GetFaces();
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++) {
if (TriMesh->RayCallback == null ||
TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
Faces[i].mU, Faces[i].mV)) {
const int& TriIndex = Faces[i].mFaceID;
if (!Callback(TriMesh, RayGeom, TriIndex)) {
continue;
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dVector3 dv[3];
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3 vu;
vu[0] = dv[1][0] - dv[0][0];
vu[1] = dv[1][1] - dv[0][1];
vu[2] = dv[1][2] - dv[0][2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = dv[2][0] - dv[0][0];
vv[1] = dv[2][1] - dv[0][1];
vv[2] = dv[2][2] - dv[0][2];
vv[3] = REAL(0.0);
dCROSS(Contact->normal, =, vv, vu); // Reversed
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (dSafeNormalize3(Contact->normal))
{
// No sense to save on single type conversion in algorithm of this size.
// If there would be a custom typedef for distance type it could be used
// instead of dReal. However using float directly is the loss of abstraction
// and possible loss of precision in future.
/*float*/ dReal T = Faces[i].mDistance;
Contact->pos[0] = Origin[0] + (Direction[0] * T);
Contact->pos[1] = Origin[1] + (Direction[1] * T);
Contact->pos[2] = Origin[2] + (Direction[2] * T);
Contact->pos[3] = REAL(0.0);
Contact->depth = T;
Contact->g1 = TriMesh;
Contact->g2 = RayGeom;
Contact->side1 = TriIndex;
Contact->side2 = -1;
OutTriCount++;
// Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
if (OutTriCount >= (Flags & NUMC_MASK)) {
break;
}
}
}
}
return OutTriCount;
}
// 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;
}
int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
{
dIASSERT( skip >= (int)sizeof( dContactGeom ) );
dIASSERT( o1->type == dTriMeshClass );
dIASSERT( o2->type == dPlaneClass );
dIASSERT ((flags & NUMC_MASK) >= 1);
// Alias pointers to the plane and trimesh
dxTriMesh* trimesh = (dxTriMesh*)( o1 );
dVector4 plane;
dGeomPlaneGetParams(o2, plane);
o1 -> recomputeAABB();
o2 -> recomputeAABB();
//Find collision
GDYNAMIC_ARRAY collision_result;
GIM_CREATE_TRIMESHPLANE_CONTACTS(collision_result);
gim_trimesh_plane_collisionODE(&trimesh->m_collision_trimesh,plane,&collision_result);
if(collision_result.m_size == 0 )
{
GIM_DYNARRAY_DESTROY(collision_result);
return 0;
}
unsigned int contactcount = collision_result.m_size;
unsigned int contactmax = (unsigned int)(flags & NUMC_MASK);
if (contactcount > contactmax)
{
contactcount = contactmax;
}
dContactGeom* pcontact;
vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f,collision_result);
for(unsigned int i = 0; i < contactcount; i++ )
{
pcontact = SAFECONTACT(flags, contacts, i, skip);
pcontact->pos[0] = (*planecontact_results)[0];
pcontact->pos[1] = (*planecontact_results)[1];
pcontact->pos[2] = (*planecontact_results)[2];
pcontact->pos[3] = REAL(1.0);
pcontact->normal[0] = plane[0];
pcontact->normal[1] = plane[1];
pcontact->normal[2] = plane[2];
pcontact->normal[3] = 0;
pcontact->depth = (*planecontact_results)[3];
pcontact->g1 = o1;
pcontact->g2 = o2;
planecontact_results++;
}
GIM_DYNARRAY_DESTROY(collision_result);
return (int)contactcount;
}
int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
{
dIASSERT( skip >= (int)sizeof( dContactGeom ) );
dIASSERT( o1->type == dTriMeshClass );
dIASSERT( o2->type == dPlaneClass );
dIASSERT ((flags & NUMC_MASK) >= 1);
// Alias pointers to the plane and trimesh
dxTriMesh* trimesh = (dxTriMesh*)( o1 );
dxPlane* plane = (dxPlane*)( o2 );
int contact_count = 0;
// Cache the maximum contact count.
const int contact_max = ( flags & NUMC_MASK );
// Cache trimesh position and rotation.
const dVector3& trimesh_pos = *(const dVector3*)dGeomGetPosition( trimesh );
const dMatrix3& trimesh_R = *(const dMatrix3*)dGeomGetRotation( trimesh );
//
// For all triangles.
//
// Cache the triangle count.
const int tri_count = trimesh->Data->Mesh.GetNbTriangles();
VertexPointers VP;
ConversionArea VC;
dReal alpha;
dVector3 vertex;
#if !defined(dSINGLE) || 1
dVector3 int_vertex; // Intermediate vertex for double precision mode.
#endif // dSINGLE
// For each triangle
for ( int t = 0; t < tri_count; ++t )
{
// Get triangle, which should also use callback.
trimesh->Data->Mesh.GetTriangle( VP, t, VC);
// For each vertex.
for ( int v = 0; v < 3; ++v )
{
//
// Get Vertex
//
#if defined(dSINGLE) && 0 // Always assign via intermediate array as otherwise it is an incapsulation violation
dMULTIPLY0_331( vertex, trimesh_R, (float*)( VP.Vertex[ v ] ) );
#else // dDOUBLE || 1
// OPCODE data is in single precision format.
int_vertex[ 0 ] = VP.Vertex[ v ]->x;
int_vertex[ 1 ] = VP.Vertex[ v ]->y;
int_vertex[ 2 ] = VP.Vertex[ v ]->z;
dMULTIPLY0_331( vertex, trimesh_R, int_vertex );
#endif // dSINGLE/dDOUBLE
vertex[ 0 ] += trimesh_pos[ 0 ];
vertex[ 1 ] += trimesh_pos[ 1 ];
vertex[ 2 ] += trimesh_pos[ 2 ];
//
// Collision?
//
// If alpha < 0 then point is if front of plane. i.e. no contact
// If alpha = 0 then the point is on the plane
alpha = plane->p[ 3 ] - dDOT( plane->p, vertex );
// If alpha > 0 the point is behind the plane. CONTACT!
if ( alpha > 0 )
{
// Alias the contact
dContactGeom* contact = SAFECONTACT( flags, contacts, contact_count, skip );
contact->pos[ 0 ] = vertex[ 0 ];
contact->pos[ 1 ] = vertex[ 1 ];
contact->pos[ 2 ] = vertex[ 2 ];
contact->normal[ 0 ] = plane->p[ 0 ];
contact->normal[ 1 ] = plane->p[ 1 ];
contact->normal[ 2 ] = plane->p[ 2 ];
contact->depth = alpha;
contact->g1 = plane;
contact->g2 = trimesh;
++contact_count;
// All contact slots are full?
if ( contact_count >= contact_max )
return contact_count; // <=== STOP HERE
}
}
}
// Return contact count.
return contact_count;
}
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
dxTriMesh* TriMesh = (dxTriMesh*)g1;
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
RayCollider& Collider = TriMesh->_RayCollider;
dReal Length = dGeomRayGetLength(RayGeom);
int FirstContact, BackfaceCull;
dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
int ClosestHit = dGeomRayGetClosestHit(RayGeom);
Collider.SetFirstContact(FirstContact != 0);
Collider.SetClosestHit(ClosestHit != 0);
Collider.SetCulling(BackfaceCull != 0);
Collider.SetMaxDist(Length);
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
/* Make Ray */
Ray WorldRay;
WorldRay.mOrig.x = Origin[0];
WorldRay.mOrig.y = Origin[1];
WorldRay.mOrig.z = Origin[2];
WorldRay.mDir.x = Direction[0];
WorldRay.mDir.y = Direction[1];
WorldRay.mDir.z = Direction[2];
/* Intersect */
Matrix4x4 amatrix;
int TriCount = 0;
if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
TriCount = TriMesh->Faces.GetNbFaces();
}
if (TriCount == 0) {
return 0;
}
const CollisionFace* Faces = TriMesh->Faces.GetFaces();
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++) {
if (OutTriCount == (Flags & 0xffff)) {
break;
}
if (TriMesh->RayCallback == null ||
TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
Faces[i].mU, Faces[i].mV)) {
const int& TriIndex = Faces[i].mFaceID;
if (!Callback(TriMesh, RayGeom, TriIndex)) {
continue;
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dVector3 dv[3];
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
float T = Faces[i].mDistance;
Contact->pos[0] = Origin[0] + (Direction[0] * T);
Contact->pos[1] = Origin[1] + (Direction[1] * T);
Contact->pos[2] = Origin[2] + (Direction[2] * T);
Contact->pos[3] = REAL(0.0);
dVector3 vu;
vu[0] = dv[1][0] - dv[0][0];
vu[1] = dv[1][1] - dv[0][1];
vu[2] = dv[1][2] - dv[0][2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = dv[2][0] - dv[0][0];
vv[1] = dv[2][1] - dv[0][1];
vv[2] = dv[2][2] - dv[0][2];
vv[3] = REAL(0.0);
dCROSS(Contact->normal, =, vv, vu); // Reversed
dNormalize3(Contact->normal);
Contact->depth = T;
Contact->g1 = TriMesh;
Contact->g2 = RayGeom;
OutTriCount++;
}
}
return OutTriCount;
}
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;
}
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
SphereCollider& Collider = TriMesh->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
Sphere.mCenter.x = Position[0];
Sphere.mCenter.y = Position[1];
Sphere.mCenter.z = Position[2];
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(dxTriMesh::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 & 0xffff)){
break;
}
const int& TriIndex = Triangles[i];
dVector3 dv[3];
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
vu[0] = v1[0] - v0[0];
vu[1] = v1[1] - v0[1];
vu[2] = v1[2] - v0[2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = v2[0] - v0[0];
vv[1] = v2[1] - v0[1];
vv[2] = v2[2] - v0[2];
vv[3] = REAL(0.0);
dReal Depth;
float u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesnt hit triangle
}
dReal w = REAL(1.0) - u - v;
if (Depth < REAL(0.0)){
Depth = REAL(0.0);
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
Contact->pos[3] = REAL(0.0);
dVector4 Plane;
dCROSS(Plane, =, vv, vu); // Reversed
Plane[3] = dDOT(Plane, v0); // Using normal as plane.
dReal Area = dSqrt(dDOT(Plane, Plane)); // We can use this later
Plane[0] /= Area;
Plane[1] /= Area;
Plane[2] /= Area;
Plane[3] /= Area;
Contact->normal[0] = Plane[0];
Contact->normal[1] = Plane[1];
Contact->normal[2] = Plane[2];
Contact->normal[3] = REAL(0.0);
Contact->depth = Depth;
//Contact->g1 = TriMesh;
//Contact->g2 = SphereGeom;
OutTriCount++;
}
#ifdef MERGECONTACTS // Merge all contacts into 1
if (OutTriCount != 0){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
if (OutTriCount != 1){
Contact->normal[0] *= Contact->depth;
Contact->normal[1] *= Contact->depth;
Contact->normal[2] *= Contact->depth;
Contact->normal[3] *= Contact->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->pos[0] += TempContact->pos[0];
Contact->pos[1] += TempContact->pos[1];
Contact->pos[2] += TempContact->pos[2];
Contact->pos[3] += TempContact->pos[3];
Contact->normal[0] += TempContact->normal[0] * TempContact->depth;
Contact->normal[1] += TempContact->normal[1] * TempContact->depth;
Contact->normal[2] += TempContact->normal[2] * TempContact->depth;
Contact->normal[3] += TempContact->normal[3] * TempContact->depth;
}
Contact->pos[0] /= OutTriCount;
Contact->pos[1] /= OutTriCount;
Contact->pos[2] /= OutTriCount;
Contact->pos[3] /= OutTriCount;
// Remember to divide in square space.
Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount);
dNormalize3(Contact->normal);
}
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
return 1;
}
else return 0;
#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
if (OutTriCount != 0){
if (OutTriCount != 1){
dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal;
Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Normal[0] += Contact->normal[0] * Contact->depth;
Normal[1] += Contact->normal[1] * Contact->depth;
Normal[2] += Contact->normal[2] * Contact->depth;
Normal[3] += Contact->normal[3] * Contact->depth;
}
dNormalize3(Normal);
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->normal[0] = Normal[0];
Contact->normal[1] = Normal[1];
Contact->normal[2] = Normal[2];
Contact->normal[3] = Normal[3];
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
}
else{
SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh;
SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom;
}
return OutTriCount;
}
else return 0;
#else //MERGECONTACTNORMALS // Just gather penetration depths and return
for (int i = 0; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
//Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal));
/*Contact->normal[0] /= Contact->depth;
Contact->normal[1] /= Contact->depth;
Contact->normal[2] /= Contact->depth;
Contact->normal[3] /= Contact->depth;*/
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
}
return OutTriCount;
#endif // MERGECONTACTS
}
else return 0;
int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
{
dIASSERT( skip >= (int)sizeof( dContactGeom ) );
dIASSERT( o1->type == dTriMeshClass );
dIASSERT( o2->type == dPlaneClass );
dIASSERT ((flags & NUMC_MASK) >= 1);
// Alias pointers to the plane and trimesh
dxTriMesh* trimesh = (dxTriMesh*)( o1 );
dxPlane* plane = (dxPlane*)( o2 );
int contact_count = 0;
// Cache the maximum contact count.
const int contact_max = ( flags & NUMC_MASK );
// Cache trimesh position and rotation.
const dVector3& trimesh_pos = *(const dVector3*)dGeomGetPosition( trimesh );
const dMatrix3& trimesh_R = *(const dMatrix3*)dGeomGetRotation( trimesh );
//
// For all triangles.
//
VertexPointersEx VPE;
VertexPointers &VP = VPE.vp;
ConversionArea VC;
dReal alpha;
dVector3 vertex;
#if !defined(dSINGLE) || 1
dVector3 int_vertex; // Intermediate vertex for double precision mode.
#endif // dSINGLE
const unsigned uiTLSKind = trimesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == plane->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
VertexUseCache &vertex_use_cache = pccColliderCache->VertexUses;
// Reallocate vertex use cache if necessary
const int vertex_count = trimesh->Data->Mesh.GetNbVertices();
const bool cache_status = vertex_use_cache.ResizeAndResetVertexUSEDFlags(vertex_count);
// Cache the triangle count.
const int tri_count = trimesh->Data->Mesh.GetNbTriangles();
// For each triangle
for ( int t = 0; t < tri_count; ++t )
{
// Get triangle, which should also use callback.
bool ex_avail = trimesh->Data->Mesh.GetExTriangle( VPE, t, VC);
// For each vertex.
for ( int v = 0; v < 3; ++v )
{
// point already used ?
if (cache_status && ex_avail)
{
unsigned VIndex = VPE.Index[v];
if (vertex_use_cache.GetVertexUSEDFlag(VIndex))
continue;
// mark this point as used
vertex_use_cache.SetVertexUSEDFlag(VIndex);
}
//
// Get Vertex
//
#if defined(dSINGLE) && 0 // Always assign via intermediate array as otherwise it is an incapsulation violation
dMultiply0_331( vertex, trimesh_R, (float*)( VP.Vertex[ v ] ) );
#else // dDOUBLE || 1
// OPCODE data is in single precision format.
int_vertex[ 0 ] = VP.Vertex[ v ]->x;
int_vertex[ 1 ] = VP.Vertex[ v ]->y;
int_vertex[ 2 ] = VP.Vertex[ v ]->z;
dMultiply0_331( vertex, trimesh_R, int_vertex );
#endif // dSINGLE/dDOUBLE
vertex[ 0 ] += trimesh_pos[ 0 ];
vertex[ 1 ] += trimesh_pos[ 1 ];
vertex[ 2 ] += trimesh_pos[ 2 ];
//
// Collision?
//
// If alpha < 0 then point is if front of plane. i.e. no contact
// If alpha = 0 then the point is on the plane
alpha = plane->p[ 3 ] - dCalcVectorDot3( plane->p, vertex );
// If alpha > 0 the point is behind the plane. CONTACT!
if ( alpha > 0 )
{
// Alias the contact
dContactGeom* contact = SAFECONTACT( flags, contacts, contact_count, skip );
contact->pos[ 0 ] = vertex[ 0 ];
contact->pos[ 1 ] = vertex[ 1 ];
contact->pos[ 2 ] = vertex[ 2 ];
contact->normal[ 0 ] = plane->p[ 0 ];
contact->normal[ 1 ] = plane->p[ 1 ];
contact->normal[ 2 ] = plane->p[ 2 ];
contact->depth = alpha;
contact->g1 = trimesh;
contact->g2 = plane;
contact->side1 = t;
contact->side2 = -1;
++contact_count;
// All contact slots are full?
if ( contact_count >= contact_max )
return contact_count; // <=== STOP HERE
}
}
}
// Return contact count.
return contact_count;
}
int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride)
{
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (g2->type == dTriMeshClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh1 = (dxTriMesh*) g1;
dxTriMesh* TriMesh2 = (dxTriMesh*) g2;
//Create contact list
GDYNAMIC_ARRAY trimeshcontacts;
GIM_CREATE_CONTACT_LIST(trimeshcontacts);
g1 -> recomputeAABB();
g2 -> recomputeAABB();
//Collide trimeshes
gim_trimesh_trimesh_collision(&TriMesh1->m_collision_trimesh,&TriMesh2->m_collision_trimesh,&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 maxcontacts = (unsigned)(Flags & NUMC_MASK);
if (contactcount > maxcontacts)
{
contactcount = maxcontacts;
}
dContactGeom* pcontact;
unsigned i;
for (i=0;i<contactcount;i++)
{
pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
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 = g1;
pcontact->g2 = g2;
pcontact->side1 = ptrimeshcontacts->m_feature1;
pcontact->side2 = ptrimeshcontacts->m_feature2;
ptrimeshcontacts++;
}
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return (int)contactcount;
}
