NEINLINE neBool SameSide(const neV3 & p1, const neV3 & p2, const neV3 & a, const neV3 & edge)
{
neV3 cp1 = edge.Cross(p1 - a);
neV3 cp2 = edge.Cross(p2 - a);
f32 dot = cp1.Dot(cp2);
return (dot >= 0.0f);
}
NEINLINE void ApplyCollisionImpulseFast(neRigidBody_ * rb, const neV3 & impulse, const neV3 & contactPoint, s32 currentRecord, neBool immediate = true)
{
neV3 dv, da;
dv = impulse * rb->oneOnMass;
da = contactPoint.Cross(impulse);
// if (immediate)
rb->Derive().linearVel += dv;
// else
// {
// rb->totalDV += dv;
// rb->impulseCount++;
// }
// rb->dvRecord[currentRecord] += dv;
// neV3 dav = rb->Derive().Iinv * da;
// rb->davRecord[currentRecord] += dav;
// if (immediate)
{
rb->State().angularMom += da;
rb->Derive().angularVel = rb->Derive().Iinv * rb->State().angularMom;
}
// else
// {
// rb->totalDA += da;
//
// rb->twistCount++;
// }
}
void FindMinMaxBound(neTriangleTree * tree, neSimpleArray<s32>& triIndex, neV3 & minBound, neV3 & maxBound)
{
s32 i;
minBound.Set(1.0e6f, 1.0e6f, 1.0e6f);
maxBound.Set(-1.0e6f, -1.0e6f, -1.0e6f);
for(i = 0; i < triIndex.GetUsedCount(); i++)
{
neTriangle_ & t = tree->triangles[triIndex[i]];
for (s32 j = 0; j < 3; j++)
{
minBound.SetMin(minBound, tree->vertices[t.indices[j]]);
maxBound.SetMax(maxBound, tree->vertices[t.indices[j]]);
}
}
}
void ChooseAxis(neV3 & x, neV3 & y, const neV3 & normal)
{
neV3 mag;
mag[0] = neAbs(normal[0]);
mag[1] = neAbs(normal[1]);
mag[2] = neAbs(normal[2]);
if (mag[0] > mag[1])
{
if (mag[0] > mag[2])
{
x[0] = (normal[1] + normal[2]) / normal[0] * -1.0f;
x[1] = 1.0f;
x[2] = 1.0f;
}
else
{
x[2] = (normal[0] + normal[1]) / normal[2] * -1.0f;
x[0] = 1.0f;
x[1] = 1.0f;
}
}
else if (mag[1] > mag[2])
{
x[1] = (normal[0] + normal[2]) / normal[1] * -1.0f;
x[0] = 1.0f;
x[2] = 1.0f;
}
else
{
x[2] = (normal[0] + normal[1]) / normal[2] * -1.0f;
x[0] = 1.0f;
x[1] = 1.0f;
}
x.Normalize();
y = normal.Cross(x);
}
NEINLINE bool BoxTestParam::MeasurePlanePenetration(ConvexTestResult & result, const neV3 & normal, f32 d)
{
f32 dot = normal.Dot(trans->pos);
f32 penetrated = dot - d;
neV3 contactPoint = trans->pos;
neV3 contactNormal;
if (penetrated < 0.0f)
{
contactNormal = normal * -1.0f;
}
else
{
contactNormal = normal;
penetrated *= -1.0f;
}
neV3 progression = contactNormal * radii;
neV3 sign;
sign[0] = progression[0] > 0.0f ? 1.0f: -1.0f;
sign[1] = progression[1] > 0.0f ? 1.0f: -1.0f;
sign[2] = progression[2] > 0.0f ? 1.0f: -1.0f;
penetrated += (progression[0] * sign[0]);
penetrated += (progression[1] * sign[1]);
penetrated += (progression[2] * sign[2]);
contactPoint -= (radii[0] * sign[0]);
contactPoint -= (radii[1] * sign[1]);
contactPoint -= (radii[2] * sign[2]);
if (penetrated < 0.0f)
return false;
if (penetrated < result.depth)
{
result.depth = penetrated;
result.contactA = contactPoint;
result.contactB = contactPoint + contactNormal * penetrated;//need to project point onto triangle face
result.valid = true;
result.contactNormal = contactNormal;
//ChooseAxis(result.contactX, result.contactY, result.contactNormal);
}
return true;
}
void neTreeNode::SelectBound(const neV3 & com, neV3 & minBound, neV3 & maxBound, s32 sector)
{
switch (sector)
{
case 0:
minBound.Set(bounds[0][0], bounds[1][0], bounds[2][0]);
maxBound.Set(com[0], bounds[1][1], com[2]);
break;
case 1:
minBound.Set(com[0], bounds[1][0], bounds[2][0]);
maxBound.Set(bounds[0][1], bounds[1][1], com[2]);
break;
case 2:
minBound.Set(com[0], bounds[1][0], com[2]);
maxBound.Set(bounds[0][1], bounds[1][1], bounds[2][1]);
break;
case 3:
minBound.Set(bounds[0][0], bounds[1][0], com[2]);
maxBound.Set(com[0], bounds[1][1], bounds[2][1]);
break;
}
}
bool TestDCDTri(ConvexTestResult & res, TConvex & convexA, neT3 & transA, const neV3 & insidePoint)
{
res.valid = false;
neV3 aPoint = transA.pos;
neV3 av; av.Set(0.1f);
aPoint += av;
neV3 bPoint = insidePoint;
BigC = aPoint - bPoint;
BigCLength = BigC.Length();
neV3 * aVertArray = NULL, * bVertArray = NULL;
TConvex dummyB; dummyB.type = TConvex::TRIANGLE;
if (convexA.type == TConvex::BOX)
{
for (s32 i = 0; i < BOX_NUM_VERTS; i++)
{
_boxVertexPosP[i] = _boxVertexPos0[i] * convexA.as.box.boxSize;
}
aVertArray = _boxVertexPosP;
}
neT3 transB; transB.SetIdentity();
SearchResult srBoxFaceTriVert(convexA, &transA, dummyB, &transB, aVertArray, bVertArray);
neBool assigned;
neBool r = srBoxFaceTriVert.SearchFV(0, assigned);
if (!r)
{return false;}
/* bPoint = insidePoint;// + _triNormals[1];
BigC = bPoint - aPoint;
BigCLength = BigC.Length();
*/
BigC *= -1.0f;
SearchResult srBoxVertTriFace(dummyB, &transB, convexA, &transA, bVertArray, aVertArray);
if (!(r = srBoxVertTriFace.TestFace(0, assigned)))
return false;
//BigC *= -1.0f;
/*
bPoint = insidePoint + _triNormals[0];
BigC = bPoint - aPoint;
BigCLength = BigC.Length();
*/
neBool assigned2;
if (!(r = srBoxVertTriFace.TestFace(1, assigned2)))
return false;
assigned |= assigned2;
BigC *= -1.0f;
/*
bPoint = insidePoint;
BigC = aPoint - bPoint;
BigCLength = BigC.Length();
*/
neBool need2Swap = false;
SearchResult srBoxTriEE(convexA, &transA, dummyB, &transB, aVertArray, bVertArray);
s32 eeflag = 0;
s32 pindex, qindex;
if (srBoxVertTriFace.dMax > srBoxFaceTriVert.dMax)
{
need2Swap = true;
srBoxTriEE.dMax = srBoxVertTriFace.dMax;
eeflag = 1;
pindex = srBoxVertTriFace.indexB; // vertex of Convex
qindex = srBoxVertTriFace.indexA; // face of Triangle
}
else
{
srBoxTriEE.dMax = srBoxFaceTriVert.dMax;
pindex = srBoxFaceTriVert.indexA; // face of Convex
qindex = srBoxFaceTriVert.indexB; // vertex of Triangle
}
//BigC *= -1.0f;
if (!srBoxTriEE.SearchEETri(eeflag, pindex, qindex, assigned))
{
return false;
}
if (!assigned)
{
FV_Backup:
if (!need2Swap)
{
ASSERT(srBoxFaceTriVert.typeA == SearchResult::FACE && srBoxFaceTriVert.typeB == SearchResult::VERTEX);
res.valid = true;
res.contactNormal = srBoxFaceTriVert.face.normal * -1.0f;
res.depth = srBoxFaceTriVert.face.k;
res.contactB = srBoxFaceTriVert.objB.GetVertWorld(srBoxFaceTriVert.indexB);
res.contactA = res.contactB + srBoxFaceTriVert.face.normal * srBoxFaceTriVert.face.k;
}
else
{
ASSERT(srBoxVertTriFace.typeA == SearchResult::FACE && srBoxVertTriFace.typeB == SearchResult::VERTEX);
res.valid = true;
res.contactNormal = srBoxVertTriFace.face.normal;
res.depth = srBoxVertTriFace.face.k;
res.contactA = srBoxVertTriFace.objB.GetVertWorld(srBoxVertTriFace.indexB);
res.contactB = res.contactA + srBoxVertTriFace.face.normal * srBoxVertTriFace.face.k;
}
}
else
{
ASSERT(srBoxTriEE.typeA == SearchResult::EDGE &&
srBoxTriEE.typeB == SearchResult::EDGE);
neV3 edgeA[2];
neV3 edgeB[2];
srBoxTriEE.objA.GetWorldEdgeVerts(srBoxTriEE.indexA, edgeA[0], edgeA[1]);
srBoxTriEE.objB.GetWorldEdgeVerts(srBoxTriEE.indexB, edgeB[0], edgeB[1]);
bool r = CalcContactEE(edgeA[0], edgeA[1], edgeB[0], edgeB[1], res.contactA, res.contactB);
if (r)
{
if (srBoxTriEE.face.k > 0.0f)
{
res.contactNormal = srBoxTriEE.face.normal * -1.0f;
res.depth = srBoxTriEE.face.k;
}
else
{
res.contactNormal = srBoxTriEE.face.normal;
res.depth = srBoxTriEE.face.k * -1.0f;
}
res.valid = true;
}
else
{
//return false;
goto FV_Backup;
}
}
return true;
}
bool TestDCD(neCollisionResult & result, TConvex & convexA, neT3 & transA, TConvex & convexB, neT3 & transB, const neV3 & backupVector)
{
_num_edge_test = 0;
_num_face_test = 0;
result.penetrate = false;
neV3 aPoint = transA.pos;
neV3 av; av.Set(0.1f);
aPoint += av;
neV3 bPoint = transB.pos;
av.Set(0.2f);
bPoint += av;
BigC = aPoint - bPoint;
BigCLength = BigC.Length();
neV3 * aVertArray = NULL, * bVertArray = NULL;
if (convexA.type == TConvex::BOX)
{
for (s32 i = 0; i < BOX_NUM_VERTS; i++)
{
_boxVertexPosP[i] = _boxVertexPos0[i] * convexA.as.box.boxSize;
}
aVertArray = _boxVertexPosP;
}
if (convexB.type == TConvex::BOX)
{
for (s32 i = 0; i < BOX_NUM_VERTS; i++)
{
_boxVertexPosQ[i] = _boxVertexPos0[i] * convexB.as.box.boxSize;
}
bVertArray = _boxVertexPosQ;
}
SearchResult srFV(convexA, &transA, convexB, &transB, aVertArray, bVertArray);
neBool showDebug = 0;
neBool showDebug2 = (srFV.objA.mesh.numVerts > 8 && srFV.objB.mesh.numVerts > 8);
neBool assigned;
neBool res = srFV.SearchFV(0, assigned);
if (!res)
{
if (showDebug)
{TOKAMAK_OUTPUT_2("%d, %d \n", _num_face_test, _num_edge_test);}
return false;
}
SearchResult srVF(convexB, &transB, convexA, &transA, bVertArray, aVertArray);
srVF.dMax = srFV.dMax;
BigC *= -1.0f;
s32 whichF = srFV.objB.mesh.edges[srFV.objB.mesh.verts[srFV.indexB].neighbourEdges[0]].f1;
res = srVF.SearchFV(whichF, assigned);
if (!res)
{
if (showDebug)
{TOKAMAK_OUTPUT_2("%d, %d \n", _num_face_test, _num_edge_test);}
return false;
}
bool need2Swap = false;
SearchResult srEE(convexA, &transA, convexB, &transB, aVertArray, bVertArray);
s32 eeflag = 0;
s32 pindex, qindex;
if (srVF.dMax > srFV.dMax)
{
need2Swap = true;
srEE.dMax = srVF.dMax;
eeflag = 1;
pindex = srVF.indexB;
qindex = srVF.indexA;
}
else
{
srEE.dMax = srFV.dMax;
pindex = srFV.indexA;
qindex = srFV.indexB;
}
BigC *= -1.0f;
if (!srEE.SearchEE(eeflag, pindex, qindex, assigned))
{
if (showDebug)
{TOKAMAK_OUTPUT_2("%d, %d \n", _num_face_test, _num_edge_test);}
return false;
}
if (showDebug2)
{
TOKAMAK_OUTPUT_2("%d, %d \n", _num_face_test, _num_edge_test);
}
if (!assigned)
{
if (!need2Swap)
{
ASSERT(srFV.typeA == SearchResult::FACE && srFV.typeB == SearchResult::VERTEX);
result.penetrate = true;
result.collisionFrame[2] = srFV.face.normal * -1.0f;
result.depth = srFV.face.k;
result.contactB = srFV.objB.GetVertWorld(srFV.indexB);
result.contactA = result.contactB + srFV.face.normal * srFV.face.k;
}
else
{
ASSERT(srVF.typeA == SearchResult::FACE && srVF.typeB == SearchResult::VERTEX);
result.penetrate = true;
result.collisionFrame[2] = srVF.face.normal;
result.depth = srVF.face.k;
result.contactA = srVF.objB.GetVertWorld(srVF.indexB);
result.contactB = result.contactA + srVF.face.normal * srVF.face.k;
}
}
else
{
ASSERT(srEE.typeA == SearchResult::EDGE &&
srEE.typeB == SearchResult::EDGE);
neV3 edgeA[2];
neV3 edgeB[2];
srEE.objA.GetWorldEdgeVerts(srEE.indexA, edgeA[0], edgeA[1]);
srEE.objB.GetWorldEdgeVerts(srEE.indexB, edgeB[0], edgeB[1]);
bool r = CalcContactEE(edgeA[0], edgeA[1], edgeB[0], edgeB[1], result.contactA, result.contactB);
if (r)
{
if (srEE.face.k > 0.0f)
{
result.collisionFrame[2] = srEE.face.normal * -1.0f;
result.depth = srEE.face.k;
}
else
{
result.collisionFrame[2] = srEE.face.normal;
result.depth = srEE.face.k * -1.0f;
}
result.penetrate = true;
}
else
{
return false;
}
}
return true;
}
