DenseIntVectSet& DenseIntVectSet::operator|=(const DenseIntVectSet& d) { if (m_domain.contains(d.m_domain)) { BoxIterator bit(d.m_domain); int i=0; for (bit.begin(); bit.ok(); ++bit, ++i) { if (d.m_bits[i]) m_bits.setTrue(m_domain.index(bit())); } } else if (d.m_domain.contains(m_domain)) { DenseIntVectSet newSet = d; BoxIterator bit(m_domain); int i=0; for (bit.begin(); bit.ok(); ++bit, ++i) { if (m_bits[i]) newSet.m_bits.setTrue(newSet.m_domain.index(bit())); } *this = newSet; } else { Box newDomain = minBox(m_domain, d.m_domain); DenseIntVectSet newSet(newDomain, false); newSet |= *this; newSet |= d; *this = newSet; } return *this; }
static int MakeRandomGuassianPointCloud (NewtonMesh* const mesh, dVector* const points, int count) { dVector size(0.0f); dMatrix matrix(dGetIdentityMatrix()); NewtonMeshCalculateOOBB(mesh, &matrix[0][0], &size.m_x, &size.m_y, &size.m_z); dVector minBox (matrix.m_posit - matrix[0].Scale (size.m_x) - matrix[1].Scale (size.m_y) - matrix[2].Scale (size.m_z)); dVector maxBox (matrix.m_posit + matrix[0].Scale (size.m_x) + matrix[1].Scale (size.m_y) + matrix[2].Scale (size.m_z)); size = (maxBox - minBox).Scale (0.5f); dVector origin = (maxBox + minBox).Scale (0.5f); dFloat biasExp = 10.0f; dFloat r = dSqrt (size.DotProduct3(size)); r = powf(r, 1.0f/biasExp); for (int i = 0; i < count; i++) { dVector& p = points[i]; bool test; do { p = dVector (2.0f * RandomVariable(r), 2.0f * RandomVariable(r), 2.0f * RandomVariable(r), 0.0f); dFloat len = dSqrt (p.DotProduct3(p)); dFloat scale = powf(len, biasExp) / len; p = p.Scale (scale) + origin; test = (p.m_x > minBox.m_x) && (p.m_x < maxBox.m_x) && (p.m_y > minBox.m_y) && (p.m_y < maxBox.m_y) && (p.m_z > minBox.m_z) && (p.m_z < maxBox.m_z); } while (!test); } return count; }
bool isCenterInViewport(const Frustum& frustum, const Boxf& worldBox, const Viewport& viewport) const { Vector4f center = worldBox.getCenter(); center[3] = 1.0; Vector4f mvpCenter = frustum.getMVPMatrix() * center; Vector3f mvpCenterHom = mvpCenter / mvpCenter[3]; const bool isNegXBorder = viewport[0] == 0.0f; // left const bool isPosXBorder = viewport[0] + viewport[2] == 1.0f; // left + width const bool isNegYBorder = viewport[1] == 0.0f; // top const bool isPosYBorder = viewport[1] + viewport[3] == 1.0f; // top + height Vector3f minBox(-1.0f); Vector3f maxBox(1.0f); if (isNegXBorder) minBox[0] = -infinite; if (isPosXBorder) maxBox[0] = infinite; if (isNegYBorder) minBox[1] = -infinite; if (isPosYBorder) maxBox[1] = infinite; minBox[2] = -infinite; maxBox[2] = infinite; const Boxf ndcCube(minBox, maxBox); return ndcCube.isIn(mvpCenterHom); }
IntVectSet& IntVectSet::operator&=(const IntVectSet& ivs) { if (!(minBox().intersects(ivs.minBox()))) { m_ivs.clear(); m_isdense = true; m_dense = DenseIntVectSet(); return *this; } if (m_isdense) { if (ivs.m_isdense) m_dense&=ivs.m_dense; else { convert(); m_ivs &= ivs.m_ivs; } } else { if (ivs.m_isdense) ivs.convert(); m_ivs &= ivs.m_ivs; } return *this; }
dgInt32 dgBroadPhaseDefault::ConvexCast(dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, dgFloat32& timeToImpact, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const { dgInt32 totalCount = 0; if (m_rootNode) { dgVector boxP0; dgVector boxP1; dgAssert(matrix.TestOrthogonal()); shape->CalcAABB(matrix, boxP0, boxP1); dgFloat32 distance[DG_BROADPHASE_MAX_STACK_DEPTH]; const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH]; dgVector velocA((target - matrix.m_posit) & dgVector::m_triplexMask); dgVector velocB(dgFloat32(0.0f)); dgFastRayTest ray(dgVector(dgFloat32(0.0f)), velocA); dgVector minBox(m_rootNode->m_minBox - boxP1); dgVector maxBox(m_rootNode->m_maxBox - boxP0); stackPool[0] = m_rootNode; distance[0] = ray.BoxIntersect(minBox, maxBox); totalCount = dgBroadPhase::ConvexCast(stackPool, distance, 1, velocA, velocB, ray, shape, matrix, target, timeToImpact, prefilter, userData, info, maxContacts, threadIndex); } return totalCount; }
void LoadLumberYardMesh(DemoEntityManager* const scene, const dVector& location, int shapeid) { DemoEntity* const entity = DemoEntity::LoadNGD_mesh ("lumber.ngd", scene->GetNewton(), scene->GetShaderCache()); dTree<NewtonCollision*, DemoMesh*> filter; NewtonWorld* const world = scene->GetNewton(); dFloat density = 15.0f; int defaultMaterialID = NewtonMaterialGetDefaultGroupID(scene->GetNewton()); for (DemoEntity* child = entity->GetFirst(); child; child = child->GetNext()) { DemoMesh* const mesh = (DemoMesh*)child->GetMesh(); if (mesh) { dAssert(mesh->IsType(DemoMesh::GetRttiType())); dTree<NewtonCollision*, DemoMesh*>::dTreeNode* node = filter.Find(mesh); if (!node) { // make a collision shape only for and instance dFloat* const array = mesh->m_vertex; dVector minBox(1.0e10f, 1.0e10f, 1.0e10f, 1.0f); dVector maxBox(-1.0e10f, -1.0e10f, -1.0e10f, 1.0f); for (int i = 0; i < mesh->m_vertexCount; i++) { dVector p(array[i * 3 + 0], array[i * 3 + 1], array[i * 3 + 2], 1.0f); minBox.m_x = dMin(p.m_x, minBox.m_x); minBox.m_y = dMin(p.m_y, minBox.m_y); minBox.m_z = dMin(p.m_z, minBox.m_z); maxBox.m_x = dMax(p.m_x, maxBox.m_x); maxBox.m_y = dMax(p.m_y, maxBox.m_y); maxBox.m_z = dMax(p.m_z, maxBox.m_z); } dVector size(maxBox - minBox); dMatrix offset(dGetIdentityMatrix()); offset.m_posit = (maxBox + minBox).Scale(0.5f); NewtonCollision* const shape = NewtonCreateBox(world, size.m_x, size.m_y, size.m_z, shapeid, &offset[0][0]); node = filter.Insert(shape, mesh); } // create a body and add to the world NewtonCollision* const shape = node->GetInfo(); dMatrix matrix(child->GetMeshMatrix() * child->CalculateGlobalMatrix()); matrix.m_posit += location; dFloat mass = density * NewtonConvexCollisionCalculateVolume(shape); CreateSimpleSolid(scene, mesh, mass, matrix, shape, defaultMaterialID); } } // destroy all shapes while (filter.GetRoot()) { NewtonCollision* const shape = filter.GetRoot()->GetInfo(); NewtonDestroyCollision(shape); filter.Remove(filter.GetRoot()); } delete entity; }
dgFloat32 dgBody::ConvexRayCast (const dgFastRayTest& ray, const dgCollisionInstance* const convexShape, const dgVector& shapeMinBox, const dgVector& shapeMaxBox, const dgMatrix& origin, const dgVector& shapeVeloc, OnRayCastAction filter, OnRayPrecastAction preFilter, void* const userData, dgFloat32 minT, dgInt32 threadId) const { dgVector minBox (m_minAABB - shapeMaxBox); dgVector maxBox (m_maxAABB - shapeMinBox); if (ray.BoxTest (minBox, maxBox)) { dgContactPoint contactOut; dgFloat32 t = m_collision->ConvexRayCast (convexShape, origin, shapeVeloc, minT, contactOut, preFilter, this, userData, threadId); if (t < minT) { dgAssert (t >= dgFloat32 (0.0f)); dgAssert (t <= dgFloat32 (1.1f)); minT = filter (this, contactOut.m_collision0, contactOut.m_point, contactOut.m_normal, contactOut.m_shapeId0, userData, t); } } return minT; }
static int PlaneCollisionAABBOverlapTest (void* userData, const dFloat* const box0, const dFloat* const box1) { const dInfinitePlane* const me = (dInfinitePlane*) userData; dVector minBox (box0[0], box0[1], box0[2], 0.0f); dFloat test = me->m_plane.DotProduct3(minBox) + me->m_plane.m_w; if (test > 0.0f) { return 0; } dVector maxBox (box1[0], box1[1], box1[2], 0.0f); test = me->m_plane.DotProduct3(maxBox) + me->m_plane.m_w; if (test < 0.0f) { return 0; } return 1; }
void Car::initPhysics() { NewtonWorld * nWorld = this->controller->getWorld(); NewtonCollision* collision; float mass = 900.0f; vector3df v1 = this->carNode->getBoundingBox().MinEdge; vector3df v2 = this->carNode->getBoundingBox().MaxEdge; dVector minBox(v1.X, v1.Y, v1.Z); dVector maxBox(v2.X, v2.Y, v2.Z); dVector size(maxBox - minBox); dVector origin((maxBox + minBox).Scale(0.5f)); size.m_w = 1.0f; origin.m_w = 1.0f; dMatrix offset(GetIdentityMatrix()); offset.m_posit = origin; collision = NewtonCreateBox(nWorld, size.m_x, size.m_y, size.m_z, 0, &offset[0][0]); dVector inertia; matrix4 m = this->carNode->getRelativeTransformation(); NewtonConvexHullModifierSetMatrix(collision, m.pointer()); NewtonBody * body = NewtonCreateBody(nWorld, collision, m.pointer()); NewtonBodySetUserData(body, this); NewtonConvexCollisionCalculateInertialMatrix(collision, &inertia[0], &origin[0]); NewtonBodySetMassMatrix(body, mass, mass * inertia.m_x, mass * inertia.m_y, mass * inertia.m_z); NewtonBodySetCentreOfMass(body, &origin[0]); NewtonBodySetForceAndTorqueCallback(body, applyCarMoveForce); NewtonBodySetTransformCallback(body, applyCarTransform); int matId = NewtonMaterialGetDefaultGroupID(nWorld); NewtonMaterialSetCollisionCallback(nWorld, matId, matId, this, 0, applyCarCollisionForce); NewtonReleaseCollision(nWorld, collision); this->setCarBodyAndGravity(body, dVector(0,-10,0,0)); this->setLocalCoordinates(this->createChassisMatrix()); }
static int MakeRandomPoisonPointCloud(NewtonMesh* const mesh, dVector* const points) { dVector size(0.0f); dMatrix matrix(dGetIdentityMatrix()); NewtonMeshCalculateOOBB(mesh, &matrix[0][0], &size.m_x, &size.m_y, &size.m_z); dVector minBox (matrix.m_posit - matrix[0].Scale (size.m_x) - matrix[1].Scale (size.m_y) - matrix[2].Scale (size.m_z)); dVector maxBox (matrix.m_posit + matrix[0].Scale (size.m_x) + matrix[1].Scale (size.m_y) + matrix[2].Scale (size.m_z)); size = maxBox - minBox; int xCount = int (size.m_x / POINT_DENSITY_PER_METERS) + 1; int yCount = int (size.m_y / POINT_DENSITY_PER_METERS) + 1; int zCount = int (size.m_z / POINT_DENSITY_PER_METERS) + 1; int count = 0; dFloat z0 = minBox.m_z; for (int iz = 0; (iz < zCount) && (count < MAX_POINT_CLOUD_SIZE); iz ++) { dFloat y0 = minBox.m_y; for (int iy = 0; (iy < yCount) && (count < MAX_POINT_CLOUD_SIZE); iy ++) { dFloat x0 = minBox.m_x; for (int ix = 0; (ix < xCount) && (count < MAX_POINT_CLOUD_SIZE); ix ++) { dFloat x = x0; dFloat y = y0; dFloat z = z0; x += RandomVariable(POISON_VARIANCE); y += RandomVariable(POISON_VARIANCE); z += RandomVariable(POISON_VARIANCE); points[count] = dVector (x, y, z); count ++; x0 += POINT_DENSITY_PER_METERS; } y0 += POINT_DENSITY_PER_METERS; } z0 += POINT_DENSITY_PER_METERS; } return count; }
void dgBroadPhaseDefault::ConvexRayCast(dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, OnRayCastAction filter, OnRayPrecastAction prefilter, void* const userData, dgInt32 threadId) const { if (filter && m_rootNode && shape->IsType(dgCollision::dgCollisionConvexShape_RTTI)) { dgVector boxP0; dgVector boxP1; shape->CalcAABB(shape->GetLocalMatrix() * matrix, boxP0, boxP1); //dgInt32 stack = 1; dgFloat32 distance[DG_COMPOUND_STACK_DEPTH]; const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH]; dgVector velocA((target - matrix.m_posit) & dgVector::m_triplexMask); dgFastRayTest ray(dgVector(dgFloat32(0.0f)), velocA); dgVector minBox(m_rootNode->m_minBox - boxP1); dgVector maxBox(m_rootNode->m_maxBox - boxP0); stackPool[0] = m_rootNode; distance[0] = ray.BoxIntersect(minBox, maxBox); dgBroadPhase::ConvexRayCast(stackPool, distance, 1, velocA, ray, shape, matrix, target, filter, prefilter, userData, threadId); } }
IntVectSet& IntVectSet::operator-=(const IntVectSet& ivs) { if (!(minBox().intersects(ivs.minBox()))) return *this; if (m_isdense) { if (ivs.m_isdense) m_dense-=ivs.m_dense; else { for (TreeIntVectSetIterator it(ivs.m_ivs); it.ok(); ++it) m_dense -= it(); } } else { if (ivs.m_isdense) { for (DenseIntVectSetIterator it(ivs.m_dense);it.ok(); ++it) m_ivs -= it(); } else m_ivs -= ivs.m_ivs; } return *this; }
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue (dgCollisionParamProxy& proxy, const dgVector& polyInstanceScale, const dgVector& polyInstanceInvScale) { dgAssert (proxy.m_referenceCollision->IsType (dgCollision::dgCollisionConvexShape_RTTI)); dgAssert (proxy.m_floatingCollision->IsType (dgCollision::dgCollisionConvexPolygon_RTTI)); const dgCollisionInstance* const hull = proxy.m_referenceCollision; dgAssert (this == proxy.m_floatingCollision->GetChildShape()); dgAssert (m_count); dgAssert (m_count < dgInt32 (sizeof (m_localPoly) / sizeof (m_localPoly[0]))); const dgBody* const floatingBody = proxy.m_floatingBody; const dgBody* const referenceBody = proxy.m_referenceBody; dgContact* const contactJoint = proxy.m_contactJoint; contactJoint->m_closestDistance = dgFloat32 (1.0e10f); m_normal = m_normal.CompProduct4(polyInstanceInvScale); dgAssert (m_normal.m_w == dgFloat32 (0.0f)); m_normal = m_normal.CompProduct4(m_normal.DotProduct4(m_normal).InvSqrt()); const dgVector savedFaceNormal (m_normal); for (dgInt32 i = 0; i < m_count; i ++) { m_localPoly[i] = polyInstanceScale.CompProduct4(dgVector (&m_vertex[m_vertexIndex[i] * m_stride])); dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f)); } dgVector hullOrigin (proxy.m_matrix.UntransformVector(dgVector (dgFloat32 (0.0f)))); hullOrigin = (hullOrigin - m_normal.CompProduct4(m_normal.DotProduct4(hullOrigin - m_localPoly[0]))) | dgVector::m_wOne; dgMatrix polygonMatrix; polygonMatrix[0] = m_localPoly[1] - m_localPoly[0]; polygonMatrix[0] = polygonMatrix[0].CompProduct4 (polygonMatrix[0].InvMagSqrt()); polygonMatrix[1] = m_normal; polygonMatrix[2] = polygonMatrix[0] * m_normal; polygonMatrix[3] = hullOrigin; dgAssert (polygonMatrix.TestOrthogonal()); dgMatrix savedProxyMatrix (proxy.m_matrix); proxy.m_matrix = polygonMatrix * proxy.m_matrix; dgVector floatingVeloc (floatingBody->m_veloc); dgVector referenceVeloc (referenceBody->m_veloc); const dgMatrix& hullMatrix = hull->GetGlobalMatrix(); dgVector hullRelativeVeloc (hullMatrix.UnrotateVector(referenceVeloc - floatingVeloc)); dgVector polyRelativeVeloc (proxy.m_matrix.UnrotateVector (hullRelativeVeloc)); dgVector polyBoxP0 (dgFloat32 ( 1.0e15f)); dgVector polyBoxP1 (dgFloat32 (-1.0e15f)); m_normal = polygonMatrix.UnrotateVector(m_normal); if (m_normal.DotProduct4(polyRelativeVeloc).m_x >= 0.0f) { proxy.m_matrix = savedProxyMatrix; return 0; } for (dgInt32 i = 0; i < m_count; i ++) { m_localPoly[i] = polygonMatrix.UntransformVector(m_localPoly[i]); dgAssert (m_localPoly[i].m_w == dgFloat32 (0.0f)); polyBoxP0 = polyBoxP0.GetMin (m_localPoly[i]); polyBoxP1 = polyBoxP1.GetMax (m_localPoly[i]); } dgInt32 count = 0; dgVector hullBoxP0; dgVector hullBoxP1; hull->CalcAABB (proxy.m_matrix.Inverse(), hullBoxP0, hullBoxP1); dgVector minBox (polyBoxP0 - hullBoxP1); dgVector maxBox (polyBoxP1 - hullBoxP0); dgFastRayTest ray (dgVector (dgFloat32 (0.0f)), polyRelativeVeloc); dgFloat32 distance = ray.BoxIntersect(minBox, maxBox); if (distance < dgFloat32 (1.0f)) { dgVector boxSize ((hullBoxP1 - hullBoxP0).Scale4 (dgFloat32 (0.5f))); // dgVector boxOrigin ((hullBoxP1 + hullBoxP0).Scale4 (dgFloat32 (0.5f))); // boxOrigin += polyRelativeVeloc.Scale4 (distance); dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f)))); dgVector pointInHull (hull->SupportVertex (normalInHull, NULL)); dgVector pointInPlane (proxy.m_matrix.UntransformVector (pointInHull)); dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal; dgFloat32 timeToPlane = distToPlane / (polyRelativeVeloc % m_normal); dgVector boxOrigin (pointInPlane + polyRelativeVeloc.Scale4(timeToPlane)); bool inside = true; dgInt32 i0 = m_count - 1; for (dgInt32 i = 0; i < m_count; i ++) { dgVector e (m_localPoly[i] - m_localPoly[i0]); dgVector n (m_normal * e); dgPlane plane (n, - (m_localPoly[i0] % n)); dgVector supportDist (plane.Abs().DotProduct4 (boxSize)); dgFloat32 centerDist = plane.Evalue(boxOrigin); if ((centerDist + supportDist.m_x) < dgFloat32 (0.0f)) { proxy.m_matrix = savedProxyMatrix; return 0; } if ((centerDist - supportDist.m_x) < dgFloat32 (0.0f)) { inside = false; } i0 = i; } // for the time being for the minkousky contact calculation inside = false; const dgInt32 hullId = hull->GetUserDataID(); if (inside) { dgVector normalInHull (proxy.m_matrix.RotateVector (m_normal.Scale4 (dgFloat32 (-1.0f)))); dgVector pointInHull (hull->SupportVertex (normalInHull, NULL)); dgVector p0 (proxy.m_matrix.UntransformVector (pointInHull)); dgFloat32 timetoImpact = dgFloat32 (0.0f); //dgFloat32 closestDistance = dgFloat32 (0.0f); dgAssert (0); // dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness + DG_IMPULSIVE_CONTACT_PENETRATION; dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness; if (penetration < dgFloat32 (0.0f)) { timetoImpact = penetration / (polyRelativeVeloc % m_normal); dgAssert (timetoImpact >= dgFloat32 (0.0f)); // closestDistance = -penetration; } if (timetoImpact <= proxy.m_timestep) { dgVector pointsContacts[64]; contactJoint->m_closestDistance = penetration; dgAssert (0); // dgVector point (pointInHull - normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION)); dgVector point (pointInHull); count = hull->CalculatePlaneIntersection (normalInHull, point, pointsContacts, 1.0f); dgAssert (0); // dgVector step (hullRelativeVeloc.Scale3 (timetoImpact) + normalInHull.Scale4(DG_IMPULSIVE_CONTACT_PENETRATION)); dgVector step (hullRelativeVeloc.Scale3 (timetoImpact)); penetration = dgMax (penetration, dgFloat32 (0.0f)); const dgMatrix& worldMatrix = hull->m_globalMatrix; dgContactPoint* const contactsOut = proxy.m_contacts; dgVector globalNormal (worldMatrix.RotateVector(normalInHull)); for (dgInt32 i = 0; i < count; i ++) { contactsOut[i].m_point = worldMatrix.TransformVector (pointsContacts[i] + step); contactsOut[i].m_normal = globalNormal; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; contactsOut[i].m_penetration = penetration; } } } else { dgFloat32 convexSphapeUmbra = hull->GetUmbraClipSize (); if (m_faceClipSize > convexSphapeUmbra) { BeamClipping (boxOrigin, convexSphapeUmbra); m_faceClipSize = hull->m_childShape->GetBoxMaxRadius(); } dgCollisionConvex* const convexShape = (dgCollisionConvex*) hull->m_childShape; count = convexShape->CalculateConvexCastContacts (proxy); // dgAssert (proxy.m_intersectionTestOnly || (count >= 0)); if (count >= 1) { dgContactPoint* const contactsOut = proxy.m_contacts; #if 0 if (m_closestFeatureType == 3) { for (dgInt32 i = 0; i < count; i ++) { contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } else { dgVector normal (polygonInstance->m_globalMatrix.UnrotateVector(contactsOut[0].m_normal)); if ((normal % savedFaceNormal) < dgFloat32 (0.995f)) { dgInt32 index = m_adjacentFaceEdgeNormalIndex[m_closestFeatureStartIndex]; dgVector n (&m_vertex[index * m_stride]); dgVector dir0 (n * savedFaceNormal); dgVector dir1 (n * normal); dgFloat32 projection = dir0 % dir1; if (projection <= dgFloat32 (0.0f)) { normal = n; } normal = polygonInstance->m_globalMatrix.RotateVector(normal); for (dgInt32 i = 0; i < count; i ++) { contactsOut[i].m_normal = normal; //contactsOut[i].m_userId = m_faceId; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } else { for (dgInt32 i = 0; i < count; i ++) { //contactsOut[i].m_userId = m_faceId; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } } #endif for (dgInt32 i = 0; i < count; i ++) { contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } } } proxy.m_matrix = savedProxyMatrix; return count; }
dgInt32 dgCollisionConvexPolygon::CalculateContactToConvexHullContinue(const dgWorld* const world, const dgCollisionInstance* const parentMesh, dgCollisionParamProxy& proxy) { dgAssert(proxy.m_instance0->IsType(dgCollision::dgCollisionConvexShape_RTTI)); dgAssert(proxy.m_instance1->IsType(dgCollision::dgCollisionConvexPolygon_RTTI)); dgAssert(this == proxy.m_instance1->GetChildShape()); dgAssert(m_count); dgAssert(m_count < dgInt32(sizeof (m_localPoly) / sizeof (m_localPoly[0]))); const dgBody* const body0 = proxy.m_body0; const dgBody* const body1 = proxy.m_body1; dgAssert (proxy.m_instance1->GetGlobalMatrix().TestIdentity()); dgVector relativeVelocity (body0->m_veloc - body1->m_veloc); if (m_normal.DotProduct4(relativeVelocity).GetScalar() >= 0.0f) { return 0; } dgFloat32 den = dgFloat32 (1.0f) / (relativeVelocity % m_normal); if (den > dgFloat32 (1.0e-5f)) { // this can actually happens dgAssert(0); return 0; } dgContact* const contactJoint = proxy.m_contactJoint; contactJoint->m_closestDistance = dgFloat32(1.0e10f); dgMatrix polygonMatrix; dgVector right (m_localPoly[1] - m_localPoly[0]); polygonMatrix[0] = right.CompProduct4(right.InvMagSqrt()); polygonMatrix[1] = m_normal; polygonMatrix[2] = polygonMatrix[0] * m_normal; polygonMatrix[3] = dgVector::m_wOne; dgAssert (polygonMatrix.TestOrthogonal()); dgVector polyBoxP0(dgFloat32(1.0e15f)); dgVector polyBoxP1(dgFloat32(-1.0e15f)); for (dgInt32 i = 0; i < m_count; i++) { dgVector point (polygonMatrix.UnrotateVector(m_localPoly[i])); polyBoxP0 = polyBoxP0.GetMin(point); polyBoxP1 = polyBoxP1.GetMax(point); } dgVector hullBoxP0; dgVector hullBoxP1; dgMatrix hullMatrix (polygonMatrix * proxy.m_instance0->m_globalMatrix); proxy.m_instance0->CalcAABB(hullMatrix, hullBoxP0, hullBoxP1); dgVector minBox(polyBoxP0 - hullBoxP1); dgVector maxBox(polyBoxP1 - hullBoxP0); dgVector veloc (polygonMatrix.UnrotateVector (relativeVelocity)); dgFastRayTest ray(dgVector(dgFloat32(0.0f)), veloc); dgFloat32 distance = ray.BoxIntersect(minBox, maxBox); dgInt32 count = 0; if (distance < dgFloat32(1.0f)) { bool inside = false; dgVector boxSize((hullBoxP1 - hullBoxP0).CompProduct4(dgVector::m_half)); dgVector sphereMag2 (boxSize.DotProduct4(boxSize)); boxSize = sphereMag2.Sqrt(); dgVector pointInPlane (polygonMatrix.RotateVector(hullBoxP1 + hullBoxP0).CompProduct4(dgVector::m_half)); dgFloat32 distToPlane = (m_localPoly[0] - pointInPlane) % m_normal; dgFloat32 timeToPlane0 = (distToPlane + boxSize.GetScalar()) * den; dgFloat32 timeToPlane1 = (distToPlane - boxSize.GetScalar()) * den; dgVector boxOrigin0 (pointInPlane + relativeVelocity.Scale4(timeToPlane0)); dgVector boxOrigin1 (pointInPlane + relativeVelocity.Scale4(timeToPlane1)); dgVector boxOrigin ((boxOrigin0 + boxOrigin1).CompProduct4(dgVector::m_half)); dgVector boxProjectSize (((boxOrigin0 - boxOrigin1).CompProduct4(dgVector::m_half))); sphereMag2 = boxProjectSize.DotProduct4(boxProjectSize); boxSize = sphereMag2.Sqrt(); dgAssert (boxOrigin.m_w == 0.0f); boxOrigin = boxOrigin | dgVector::m_wOne; if (!proxy.m_intersectionTestOnly) { inside = true; dgInt32 i0 = m_count - 1; for (dgInt32 i = 0; i < m_count; i++) { dgVector e(m_localPoly[i] - m_localPoly[i0]); dgVector n(m_normal * e & dgVector::m_triplexMask); dgFloat32 param = dgSqrt (sphereMag2.GetScalar() / (n.DotProduct4(n)).GetScalar()); dgPlane plane(n, -(m_localPoly[i0] % n)); dgVector p0 (boxOrigin + n.Scale4 (param)); dgVector p1 (boxOrigin - n.Scale4 (param)); dgFloat32 size0 = (plane.DotProduct4 (p0)).GetScalar(); dgFloat32 size1 = (plane.DotProduct4 (p1)).GetScalar(); if ((size0 < 0.0f) && (size1 < 0.0f)) { return 0; } if ((size0 * size1) < 0.0f) { inside = false; break; } i0 = i; } } dgFloat32 convexSphapeUmbra = dgMax (proxy.m_instance0->GetUmbraClipSize(), boxSize.GetScalar()); if (m_faceClipSize > convexSphapeUmbra) { BeamClipping(boxOrigin, convexSphapeUmbra); m_faceClipSize = proxy.m_instance0->m_childShape->GetBoxMaxRadius(); } const dgInt32 hullId = proxy.m_instance0->GetUserDataID(); if (inside & !proxy.m_intersectionTestOnly) { const dgMatrix& matrixInstance0 = proxy.m_instance0->m_globalMatrix; dgVector normalInHull(matrixInstance0.UnrotateVector(m_normal.Scale4(dgFloat32(-1.0f)))); dgVector pointInHull(proxy.m_instance0->SupportVertex(normalInHull, NULL)); dgVector p0 (matrixInstance0.TransformVector(pointInHull)); dgFloat32 timetoImpact = dgFloat32(0.0f); dgFloat32 penetration = (m_localPoly[0] - p0) % m_normal + proxy.m_skinThickness; if (penetration < dgFloat32(0.0f)) { timetoImpact = penetration / (relativeVelocity % m_normal); dgAssert(timetoImpact >= dgFloat32(0.0f)); } if (timetoImpact <= proxy.m_timestep) { dgVector contactPoints[64]; contactJoint->m_closestDistance = penetration; proxy.m_timestep = timetoImpact; proxy.m_normal = m_normal; proxy.m_closestPointBody0 = p0; proxy.m_closestPointBody1 = p0 + m_normal.Scale4(penetration); if (!proxy.m_intersectionTestOnly) { pointInHull -= normalInHull.Scale4 (DG_ROBUST_PLANE_CLIP); count = proxy.m_instance0->CalculatePlaneIntersection(normalInHull, pointInHull, contactPoints); dgVector step(relativeVelocity.Scale4(timetoImpact)); penetration = dgMax(penetration, dgFloat32(0.0f)); dgContactPoint* const contactsOut = proxy.m_contacts; for (dgInt32 i = 0; i < count; i++) { contactsOut[i].m_point = matrixInstance0.TransformVector(contactPoints[i]) + step; contactsOut[i].m_normal = m_normal; contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; contactsOut[i].m_penetration = penetration; } } } } else { m_vertexCount = dgUnsigned16 (m_count); count = world->CalculateConvexToConvexContacts(proxy); if (count >= 1) { dgContactPoint* const contactsOut = proxy.m_contacts; for (dgInt32 i = 0; i < count; i++) { contactsOut[i].m_shapeId0 = hullId; contactsOut[i].m_shapeId1 = m_faceId; } } } } return count; }
void dgCollisionDeformableSolidMesh::ResolvePositionsConstraints (dgFloat32 timestep) { dgAssert (m_myBody); dgInt32 strideInBytes = sizeof (dgVector) * m_clustersCount + sizeof (dgMatrix) * m_clustersCount + sizeof (dgMatrix) * m_particles.m_count; m_world->m_solverMatrixMemory.ExpandCapacityIfNeessesary (1, strideInBytes); dgVector* const regionCom = (dgVector*)&m_world->m_solverMatrixMemory[0]; dgMatrix* const sumQiPi = (dgMatrix*) ®ionCom[m_clustersCount]; dgMatrix* const covarianceMatrix = (dgMatrix*) &sumQiPi[m_clustersCount]; const dgFloat32* const masses = m_particles.m_unitMass; dgVector zero (dgFloat32 (0.0f)); for (dgInt32 i = 0; i < m_clustersCount; i ++) { regionCom[i] = zero; } for (dgInt32 i = 0; i < m_particles.m_count; i ++) { dgVector mass (masses[i]); const dgVector& r = m_posit[i]; const dgVector& r0 = m_shapePosit[i]; dgVector mr (r.Scale4(masses[i])); covarianceMatrix[i] = dgMatrix (r0, mr); const dgInt32 start = m_clusterPositStart[i]; const dgInt32 count = m_clusterPositStart[i + 1] - start; for (dgInt32 j = 0; j < count; j ++) { dgInt32 index = m_clusterPosit[start + j]; regionCom[index] += mr; } } for (dgInt32 i = 0; i < m_clustersCount; i ++) { dgVector mcr (regionCom[i]); regionCom[i] = mcr.Scale4 (dgFloat32 (1.0f) / m_clusterMass[i]); const dgVector& cr0 = m_clusterCom0[i]; sumQiPi[i] = dgMatrix (cr0, mcr.CompProduct4(dgVector::m_negOne)); } for (dgInt32 i = 0; i < m_particles.m_count; i ++) { const dgInt32 start = m_clusterPositStart[i]; const dgInt32 count = m_clusterPositStart[i + 1] - start; const dgMatrix& covariance = covarianceMatrix[i]; for (dgInt32 j = 0; j < count; j ++) { dgInt32 index = m_clusterPosit[start + j]; dgMatrix& QiPi = sumQiPi[index]; QiPi.m_front += covariance.m_front; QiPi.m_up += covariance.m_up; QiPi.m_right += covariance.m_right; } } dgVector beta0 (dgFloat32 (0.93f)); //dgVector beta0 (dgFloat32 (0.0f)); dgVector beta1 (dgVector::m_one - beta0); dgFloat32 stiffness = dgFloat32 (0.3f); for (dgInt32 i = 0; i < m_clustersCount; i ++) { dgMatrix& QiPi = sumQiPi[i]; dgMatrix S (QiPi * QiPi.Transpose4X4()); dgVector eigenValues; S.EigenVectors (eigenValues, m_clusterRotationInitialGuess[i]); m_clusterRotationInitialGuess[i] = S; #ifdef _DEBUG_____ dgMatrix P0 (QiPi * QiPi.Transpose4X4()); dgMatrix D (dgGetIdentityMatrix()); D[0][0] = eigenValues[0]; D[1][1] = eigenValues[1]; D[2][2] = eigenValues[2]; dgMatrix P1 (S.Transpose4X4() * D * S); dgAssert (P0.TestSymetric3x3()); dgAssert (P1.TestSymetric3x3()); dgMatrix xx (P1 * P0.Symetric3by3Inverse()); dgAssert (dgAbsf (xx[0][0] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgAssert (dgAbsf (xx[1][1] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgAssert (dgAbsf (xx[2][2] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f)); dgAssert (dgAbsf (xx[0][1]) < dgFloat32 (1.0e-3f)); dgAssert (dgAbsf (xx[0][2]) < dgFloat32 (1.0e-3f)); dgAssert (dgAbsf (xx[1][0]) < dgFloat32 (1.0e-3f)); dgAssert (dgAbsf (xx[1][2]) < dgFloat32 (1.0e-3f)); dgAssert (dgAbsf (xx[2][0]) < dgFloat32 (1.0e-3f)); dgAssert (dgAbsf (xx[2][1]) < dgFloat32 (1.0e-3f)); #endif eigenValues = eigenValues.InvSqrt(); dgMatrix m; m.m_front = S.m_front.CompProduct4(eigenValues.BroadcastX()); m.m_up = S.m_up.CompProduct4(eigenValues.BroadcastY()); m.m_right = S.m_right.CompProduct4(eigenValues.BroadcastZ()); m.m_posit = dgVector::m_wOne; dgMatrix invS (S.Transpose4X4() * m); dgMatrix R (invS * QiPi); dgMatrix A (m_clusterAqqInv[i] * QiPi); QiPi.m_front = A.m_front.CompProduct4(beta0) + R.m_front.CompProduct4(beta1); QiPi.m_up = A.m_up.CompProduct4(beta0) + R.m_up.CompProduct4(beta1); QiPi.m_right = A.m_right.CompProduct4(beta0) + R.m_right.CompProduct4(beta1); } dgVector invTimeScale (stiffness / timestep); dgVector* const veloc = m_particles.m_veloc; for (dgInt32 i = 0; i < m_particles.m_count; i ++) { const dgInt32 start = m_clusterPositStart[i]; const dgInt32 count = m_clusterPositStart[i + 1] - start; dgVector v (dgFloat32 (0.0f)); const dgVector& p = m_posit[i]; const dgVector& p0 = m_shapePosit[i]; for (dgInt32 j = 0; j < count; j ++) { dgInt32 index = m_clusterPosit[start + j]; const dgMatrix& matrix = sumQiPi[index]; dgVector gi (matrix.RotateVector(p0 - m_clusterCom0[index]) + regionCom[index]); v += ((gi - p).CompProduct4(invTimeScale).Scale4 (m_clusterWeight[index])); } veloc[i] += v; } // resolve collisions here //for now just a hack a collision plane until I get the engine up an running for (dgInt32 i = 0; i < m_particles.m_count; i ++) { dgVector p (m_basePosit + m_posit[i].m_y); if (p.m_y < dgFloat32 (0.0f)) { m_posit[i].m_y = -m_basePosit.m_y; veloc[i].m_y = dgFloat32 (0.0f); } } dgVector time (timestep); dgVector minBase(dgFloat32 (1.0e10f)); dgVector minBox (dgFloat32 (1.0e10f)); dgVector maxBox (dgFloat32 (-1.0e10f)); dgMatrix matrix (m_myBody->GetCollision()->GetGlobalMatrix().Inverse()); for (dgInt32 i = 0; i < m_particles.m_count; i ++) { m_posit[i] += veloc[i].CompProduct4 (time); m_particles.m_posit[i] = matrix.TransformVector(m_posit[i] + m_basePosit); minBase = minBase.GetMin (m_posit[i]); minBox = minBox.GetMin(m_particles.m_posit[i]); maxBox = maxBox.GetMax(m_particles.m_posit[i]); } minBase = minBase.Floor(); dgVector mask ((minBase < dgVector (dgFloat32 (0.0f))) | (minBase >= dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE)))); dgInt32 test = mask.GetSignMask(); if (test & 0x07) { dgVector offset (((minBase < dgVector (dgFloat32 (0.0f))) & dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE/2))) + ((minBase >= dgVector (dgFloat32 (DG_SOFTBODY_BASE_SIZE))) & dgVector (dgFloat32 (-DG_SOFTBODY_BASE_SIZE/2)))); m_basePosit -= offset; for (dgInt32 i = 0; i < m_particles.m_count; i ++) { m_posit[i] += offset; } } // integrate each particle by the deformation velocity, also calculate the new com // calculate the new body average velocity // if (m_myBody->m_matrixUpdate) { // myBody->m_matrixUpdate (*myBody, myBody->m_matrix, threadIndex); // } // the collision changed shape, need to update spatial structure // UpdateCollision (); // SetCollisionBBox (m_rootNode->m_minBox, m_rootNode->m_maxBox); SetCollisionBBox (minBox, maxBox); }
void DemoMesh::SpliteSegment(dListNode* const node, int maxIndexCount) { const DemoSubMesh& segment = node->GetInfo(); if (segment.m_indexCount > maxIndexCount) { dVector minBox (1.0e10f, 1.0e10f, 1.0e10f, 0.0f); dVector maxBox (-1.0e10f, -1.0e10f, -1.0e10f, 0.0f); for (int i = 0; i < segment.m_indexCount; i ++) { int index = segment.m_indexes[i]; for (int j = 0; j < 3; j ++) { minBox[j] = (m_vertex[index * 3 + j] < minBox[j]) ? m_vertex[index * 3 + j] : minBox[j]; maxBox[j] = (m_vertex[index * 3 + j] > maxBox[j]) ? m_vertex[index * 3 + j] : maxBox[j]; } } int index = 0; dFloat maxExtend = -1.0e10f; for (int j = 0; j < 3; j ++) { dFloat ext = maxBox[j] - minBox[j]; if (ext > maxExtend ) { index = j; maxExtend = ext; } } int leftCount = 0; int rightCount = 0; dFloat spliteDist = (maxBox[index ] + minBox[index]) * 0.5f; for (int i = 0; i < segment.m_indexCount; i += 3) { bool isleft = true; for (int j = 0; j < 3; j ++) { int vertexIndex = segment.m_indexes[i + j]; isleft &= (m_vertex[vertexIndex * 3 + index] < spliteDist); } if (isleft) { leftCount += 3; } else { rightCount += 3; } } dAssert (leftCount); dAssert (rightCount); dListNode* const leftNode = Append(); dListNode* const rightNode = Append(); DemoSubMesh* const leftSubMesh = &leftNode->GetInfo(); DemoSubMesh* const rightSubMesh = &rightNode->GetInfo(); leftSubMesh->AllocIndexData (leftCount); rightSubMesh->AllocIndexData (rightCount); leftSubMesh->m_textureHandle = AddTextureRef (segment.m_textureHandle); rightSubMesh->m_textureHandle = AddTextureRef (segment.m_textureHandle); leftSubMesh->m_textureName = segment.m_textureName; rightSubMesh->m_textureName = segment.m_textureName; leftCount = 0; rightCount = 0; for (int i = 0; i < segment.m_indexCount; i += 3) { bool isleft = true; for (int j = 0; j < 3; j ++) { int vertexIndex = segment.m_indexes[i + j]; isleft &= (m_vertex[vertexIndex * 3 + index] < spliteDist); } if (isleft) { leftSubMesh->m_indexes[leftCount + 0] = segment.m_indexes[i + 0]; leftSubMesh->m_indexes[leftCount + 1] = segment.m_indexes[i + 1]; leftSubMesh->m_indexes[leftCount + 2] = segment.m_indexes[i + 2]; leftCount += 3; } else { rightSubMesh->m_indexes[rightCount + 0] = segment.m_indexes[i + 0]; rightSubMesh->m_indexes[rightCount + 1] = segment.m_indexes[i + 1]; rightSubMesh->m_indexes[rightCount + 2] = segment.m_indexes[i + 2]; rightCount += 3; } } SpliteSegment(leftNode, maxIndexCount); SpliteSegment(rightNode, maxIndexCount); Remove(node); } }