// Getting data void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2){ dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); dxTriMesh* Geom = (dxTriMesh*)g; const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); dVector3 v[3]; FetchTriangle(Geom, Index, Position, Rotation, v); if (v0){ (*v0)[0] = v[0][0]; (*v0)[1] = v[0][1]; (*v0)[2] = v[0][2]; (*v0)[3] = v[0][3]; } if (v1){ (*v1)[0] = v[1][0]; (*v1)[1] = v[1][1]; (*v1)[2] = v[1][2]; (*v1)[3] = v[1][3]; } if (v2){ (*v2)[0] = v[2][0]; (*v2)[1] = v[2][1]; (*v2)[2] = v[2][2]; (*v2)[3] = v[2][3]; } }
void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){ dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); dxTriMesh* Geom = (dxTriMesh*)g; const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); dVector3 dv[3]; FetchTriangle(Geom, Index, Position, Rotation, dv); GetPointFromBarycentric(dv, u, v, Out); }
int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) { dIASSERT( skip >= (int)sizeof( dContactGeom ) ); dIASSERT( o1->type == dCylinderClass ); dIASSERT( o2->type == dTriMeshClass ); dIASSERT ((flags & NUMC_MASK) >= 1); // Main data holder sData cData; // Assign ODE stuff cData.gCylinder = o1; cData.gTrimesh = (dxTriMesh*)o2; cData.iFlags = flags; cData.iSkip = skip; cData.gContact = contact; cData.nContacts = 0; _InitCylinderTrimeshData(cData); OBBCollider& Collider = cData.gTrimesh->_OBBCollider; Point cCenter((float)cData.vCylinderPos[0],(float)cData.vCylinderPos[1],(float)cData.vCylinderPos[2]); Point cExtents((float)cData.fCylinderRadius,(float)cData.fCylinderRadius,(float)cData.fCylinderRadius); cExtents[nCYLINDER_AXIS] = (float)(cData.fCylinderSize * REAL(0.5)); Matrix3x3 obbRot; // It is a potential issue to explicitly cast to float // if custom width floating point type is introduced in OPCODE. // It is necessary to make a typedef and cast to it // (e.g. typedef float opc_float;) // However I'm not sure in what header it should be added. obbRot[0][0] = (float)cData.mCylinderRot[0]; obbRot[1][0] = (float)cData.mCylinderRot[1]; obbRot[2][0] = (float)cData.mCylinderRot[2]; obbRot[0][1] = (float)cData.mCylinderRot[4]; obbRot[1][1] = (float)cData.mCylinderRot[5]; obbRot[2][1] = (float)cData.mCylinderRot[6]; obbRot[0][2] = (float)cData.mCylinderRot[8]; obbRot[1][2] = (float)cData.mCylinderRot[9]; obbRot[2][2] = (float)cData.mCylinderRot[10]; OBB obbCapsule(cCenter,cExtents,obbRot); Matrix4x4 CapsuleMatrix; MakeMatrix(cData.vCylinderPos, cData.mCylinderRot, CapsuleMatrix); Matrix4x4 MeshMatrix; MakeMatrix(cData.vTrimeshPos, cData.mTrimeshRot, MeshMatrix); // TC results if (cData.gTrimesh->doBoxTC) { dxTriMesh::BoxTC* BoxTC = 0; for (int i = 0; i < cData.gTrimesh->BoxTCCache.size(); i++) { if (cData.gTrimesh->BoxTCCache[i].Geom == cData.gCylinder) { BoxTC = &cData.gTrimesh->BoxTCCache[i]; break; } } if (!BoxTC) { cData.gTrimesh->BoxTCCache.push(dxTriMesh::BoxTC()); BoxTC = &cData.gTrimesh->BoxTCCache[cData.gTrimesh->BoxTCCache.size() - 1]; BoxTC->Geom = cData.gCylinder; BoxTC->FatCoeff = REAL(1.0); } // Intersect Collider.SetTemporalCoherence(true); Collider.Collide(*BoxTC, obbCapsule, cData.gTrimesh->Data->BVTree, null, &MeshMatrix); } else { Collider.SetTemporalCoherence(false); Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, cData.gTrimesh->Data->BVTree, null,&MeshMatrix); } // Retrieve data int TriCount = Collider.GetNbTouchedPrimitives(); const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); if (TriCount != 0) { if (cData.gTrimesh->ArrayCallback != null) { cData.gTrimesh->ArrayCallback(cData.gTrimesh, cData.gCylinder, Triangles, TriCount); } // allocate buffer for local contacts on stack cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK)); int ctContacts0 = 0; // loop through all intersecting triangles for (int i = 0; i < TriCount; i++) { const int Triint = Triangles[i]; if (!Callback(cData.gTrimesh, cData.gCylinder, Triint)) continue; dVector3 dv[3]; FetchTriangle(cData.gTrimesh, Triint, cData.vTrimeshPos, cData.mTrimeshRot, dv); // test this triangle TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false); // fill-in tri index for generated contacts for (; ctContacts0<cData.nContacts; ctContacts0++) cData.gLocalContacts[ctContacts0].triIndex = Triint; // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) { break; } } } return _ProcessLocalContacts(cData); }
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;
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 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; }
// 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] : (uint8)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; }
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; }
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 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;