virtual int BatchAddContactGroup(const Separation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths) { for (int i=0;i<numContacts;i++) { //printf("numContacts = %i\n",numContacts); SimdVector3 normalOnBInWorld(sep.m_axis.GetX(),sep.m_axis.GetY(),sep.m_axis.GetZ()); //normalOnBInWorld.normalize(); SimdVector3 pointInWorld(positionsWorld[i].GetX(),positionsWorld[i].GetY(),positionsWorld[i].GetZ()); float depth = -depths[i]; m_manifoldResult.AddContactPoint(normalOnBInWorld,pointInWorld,depth); } return 0; }
void CustomConvexConvexPairCollision::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { #if 0 if (!m_manifoldPtr) { //swapped? m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); m_ownManifold = true; } resultOut->setPersistentManifold(m_manifoldPtr); CustomConvexShape* convex0 = (CustomConvexShape*)body0->getCollisionShape(); CustomConvexShape* convex1 = (CustomConvexShape*)body1->getCollisionShape(); float4 bodyApos; float4 bodyBpos; Quaternion bodyAquat; Quaternion bodyBquat; const btTransform& transA = body0->getWorldTransform(); const btTransform& transB = body1->getWorldTransform(); const btVector3& pA = body0->getWorldTransform().getOrigin(); const btVector3& pB = body1->getWorldTransform().getOrigin(); btQuaternion qA = body0->getWorldTransform().getRotation(); btQuaternion qB = body1->getWorldTransform().getRotation(); bodyApos.x = pA.getX(); bodyApos.y = pA.getY(); bodyApos.z = pA.getZ(); bodyApos.w = 0.f; bodyBpos.x = pB.getX(); bodyBpos.y = pB.getY(); bodyBpos.z = pB.getZ(); bodyBpos.w = 0.f; bodyAquat.x = qA.getX(); bodyAquat.y = qA.getY(); bodyAquat.z = qA.getZ(); bodyAquat.w = qA.getW(); bodyBquat.x = qB.getX(); bodyBquat.y = qB.getY(); bodyBquat.z = qB.getZ(); bodyBquat.w = qB.getW(); #define CAPACITY_CONTACTS 4 ContactPoint4 contactsOut[CAPACITY_CONTACTS]; int freeContactIndex = 0; int contactCapacity = CAPACITY_CONTACTS; float collisionMargin = 0.001f; m_manifoldPtr->refreshContactPoints(body0->getWorldTransform(),body1->getWorldTransform()); collideStraight(convex0->m_ConvexHeightField,convex1->m_ConvexHeightField, bodyApos, bodyAquat,bodyBpos,bodyBquat, contactsOut, freeContactIndex, contactCapacity, collisionMargin ); collideStraight(convex1->m_ConvexHeightField,convex0->m_ConvexHeightField, bodyBpos, bodyBquat,bodyApos,bodyAquat, contactsOut, freeContactIndex, contactCapacity, collisionMargin ); //copy points into manifold //refresh manifold btAssert(freeContactIndex<3); for (int j=0; j<freeContactIndex; j++) { int numPoints = contactsOut[j].getNPoints(); // printf("numPoints = %d\n",numPoints); for (int i=0; i<numPoints; i++) { ContactPoint4& c = contactsOut[j]; btVector3 normalOnBInWorld( c.m_worldNormal.x, c.m_worldNormal.y, c.m_worldNormal.z); btVector3 pointInWorldOnB( c.m_worldPos[i].x, c.m_worldPos[i].y, c.m_worldPos[i].z); btScalar depth = c.m_worldPos[i].w; if (depth<0) { const btVector3 deltaC = transB.getOrigin() - transA.getOrigin(); if((deltaC.dot(normalOnBInWorld))>0.0f) { normalOnBInWorld= -normalOnBInWorld; } normalOnBInWorld.normalize(); if (j) { resultOut->addContactPoint(normalOnBInWorld, pointInWorldOnB, depth); } else { resultOut->addContactPoint(normalOnBInWorld, pointInWorldOnB-normalOnBInWorld*depth, depth); } } } } #else btConvexConvexAlgorithm::processCollision(body0,body1,dispatchInfo,resultOut); #endif }