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);
}
}