f32 neCollisionResult::SolveAngular2(const neV3 & axisA, const neV3 & axisB, f32 relAV, f32 desireAV, f32 maxTorque, neFixedTimeStepSimulator * sim)
{
f32 deltaAng = desireAV - relAV;
//deltaAng *= 0.5f;
neV3 deltaAngVel, deltaLA, deltaLB;
deltaAngVel = axisA * deltaAng;
deltaLA = kInv * deltaAngVel;
neV3 torque = deltaLA * sim->oneOnCurrentTimeStep;
f32 torqueMag = torque.Length();
if (torqueMag > maxTorque && !neIsConsiderZero(neAbs(maxTorque)))
{
deltaLA = torque * (maxTorque * sim->_currentTimeStep / torqueMag);
}
neRigidBody_ * rb;
if (bodyA && (rb = bodyA->AsRigidBody()))
{
rb->SetAngMom(rb->State().angularMom + deltaLA);
rb->davRecord[rb->sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] += rb->Derive().Iinv * deltaLA;
//rb->totalDA += deltaLA;
//rb->twistCount++;
}
deltaAngVel = axisB * deltaAng;
deltaLB = kInv * deltaAngVel;
torque = deltaLB * sim->oneOnCurrentTimeStep;
torqueMag = torque.Length();
if (torqueMag > maxTorque/* && !neIsConsiderZero(maxTorque)*/)
{
deltaLB = torque * (maxTorque * sim->_currentTimeStep / torqueMag);
}
if (bodyB && (rb = bodyB->AsRigidBody()))
{
rb->SetAngMom(rb->State().angularMom - deltaLB);
rb->davRecord[rb->sim->stepSoFar % NE_RB_MAX_PAST_RECORDS] -= rb->Derive().Iinv * deltaLB;
//rb->totalDA -= deltaLB;
//rb->twistCount++;
}
return 0.0f;
}
f32 neCollisionResult::SolveConstraint(neFixedTimeStepSimulator * sim)
{
neV3 impulse;
f32 ret = 0.0f;
neV3 pII;
if (neIsConsiderZero(depth))
{
pII = kInv * initRelVel;
impulse = -pII;
}
else
{
neV3 & desireVel = contactAWorld;//w2c * contactAWorld;
neV3 tmp = desireVel * CONSTRAINT_CONVERGE_FACTOR_JOINT;
f32 len = depth * CONSTRAINT_CONVERGE_FACTOR_JOINT;//tmp.Length();
if (len > 0.05f)
{
tmp = desireVel * (0.05f / len);
}
tmp *= sim->oneOnCurrentTimeStep;// * CONSTRAINT_CONVERGE_FACTOR_JOINT;
neV3 deltaU = tmp + initRelVel * CONSTRAINT_CONVERGE_FACTOR_JOINT;
impulse = kInv * deltaU * -1.0f;
}
neRigidBody_ * rb;
if (bodyA && (rb = bodyA->AsRigidBody()))
{
ApplyCollisionImpulseFast(rb, impulse, contactA, sim->currentRecord);
rb->needRecalc = true;
}
if (bodyB && (rb = bodyB->AsRigidBody()))
{
neV3 bimpulse = impulse * -1.0f;
ApplyCollisionImpulseFast(rb, bimpulse, contactB, sim->currentRecord);
rb->needRecalc = true;
}
return ret;
}
f32 neCollisionResult::SolveSlider(neFixedTimeStepSimulator * sim)
{
neV3 impulse;
if (neIsConsiderZero(finalRelativeSpeed))
{
impulse = kInv * initRelVel * -1.0f;
impulse = impulse - impulse.Dot(contactBWorld) * contactBWorld;
}
else
{
neV3 & desireVel = contactAWorld;
neV3 tmp = desireVel * CONSTRAINT_CONVERGE_FACTOR_JOINT;
f32 len = finalRelativeSpeed * CONSTRAINT_CONVERGE_FACTOR_JOINT;//tmp.Length();
if (len > 0.05f)
{
tmp = desireVel * (0.05f / len);
}
tmp *= sim->oneOnCurrentTimeStep;// * CONSTRAINT_CONVERGE_FACTOR_JOINT;
neV3 deltaU = tmp + initRelVel * CONSTRAINT_CONVERGE_FACTOR_JOINT;
impulse = kInv * deltaU * -1.0f;
f32 dot = impulse.Dot(contactBWorld);
impulse = impulse - dot * contactBWorld;
}
neRigidBody_ * rb;
if (bodyA && (rb = bodyA->AsRigidBody()))
{
ApplyCollisionImpulseFast(rb, impulse, contactA, sim->currentRecord);
rb->needRecalc = true;
}
if (bodyB && (rb = bodyB->AsRigidBody()))
{
neV3 bimpulse = impulse * -1.0f;
ApplyCollisionImpulseFast(rb, bimpulse, contactB, sim->currentRecord);
rb->needRecalc = true;
}
return 0.0f;
}
f32 funcD(const Face & face)
{
f32 k = face.k;
neV3 N;
N = face.normal;
if (face.k < 0.0f)
{
k = -face.k;
N *= -1.0f;
}
f32 den = k - N.Dot(BigC);
den = BigCLength * den;
ASSERT(!neIsConsiderZero(den));
f32 ret = -k / den;
return ret;
}
/*
*
neBool found = false;
f32 dist, ratio, factor, depth;
neV3 contact;
s32 i;
for (i = 0; i < 3; i++)
{
if (!neIsConsiderZero(sensorA.dir[i]))
{
if (sensorA.dir[i] > 0.0f)
{
if (sensorA.pos[i] > -convexB.as.box.boxSize[i])
continue;
factor = 1.0f;
}
else
{
if (sensorA.pos[i] < convexB.as.box.boxSize[i])
continue;
factor = -1.0f;
}
dist = factor * (convexB.as.box.boxSize[i] - sensorA.pos[i]);
assert(dist > 0.0f);
if (dist > neAbs(sensorA.dir[i]))
return;
ratio = dist / neAbs(sensorA.dir[i]);
contact = sensorA.pos + sensorA.dir * ratio;
s32 other1, other2;
other1 = (i + 1)%3;
other2 = (i + 2)%3;
if (contact[other1] >= convexB.as.box.boxSize[other1] || contact[other1] <= -convexB.as.box.boxSize[other1])
continue;
if (contact[other2] >= convexB.as.box.boxSize[other2] || contact[other2] <= -convexB.as.box.boxSize[other2])
continue;
found = true;
depth = (1.0f - ratio) * sensorA.length;
break;
}
else if (sensorA.pos[i] >= convexB.as.box.boxSize[i] || sensorA.pos[i] <= -convexB.as.box.boxSize[i])
{
return;
}
}
if (found)
{
sensorA.depth = depth;
sensorA.normal = transB.rot[i] * factor * -1.0f;
sensorA.contactPoint = contact;
sensorA.materialID = convexB.matIndex;
}
*/
void SensorTest(neSensor_ & sensorA, TConvex & convexB, neT3 & transB)
{
if (convexB.type == TConvex::BOX)
{
int nearDim = -1;
int farDim = -1;
// set Tnear = - infinity, Tfar = infinity
// For each pair of planes P associated with X, Y, and Z do:
// (example using X planes)
// if direction Xd = 0 then the ray is parallel to the X planes, so
// if origin Xo is not between the slabs ( Xo < Xl or Xo > Xh) then return false
// else, if the ray is not parallel to the plane then
// begin
// compute the intersection distance of the planes
// T1 = (Xl - Xo) / Xd
// T2 = (Xh - Xo) / Xd
// If T1 > T2 swap (T1, T2) /* since T1 intersection with near plane */
// If T1 > Tnear set Tnear =T1 /* want largest Tnear */
// If T2 < Tfar set Tfar="T2" /* want smallest Tfar */
// If Tnear > Tfar box is missed so return false
// If Tfar < 0 box is behind ray return false end
float tNear = -1.0e6;
float tFar = 1.0e6;
for (int i = 0; i < 3; i++)
{
if (neIsConsiderZero(sensorA.dir[i]))
{
if (sensorA.pos[i] < -convexB.as.box.boxSize[i] ||
sensorA.pos[i] > convexB.as.box.boxSize[i])
{
return;
}
}
float t1 = (-convexB.as.box.boxSize[i] - sensorA.pos[i]) / sensorA.dir[i];
float t2 = (convexB.as.box.boxSize[i] - sensorA.pos[i]) / sensorA.dir[i];
float tt;
if (t1 > t2)
{
tt = t1;
t1 = t2;
t2 = tt;
}
if (t1 > tNear)
{
tNear = t1;
nearDim = i;
}
if (t2 < tFar)
{
tFar = t2;
farDim = i;
}
if (tNear > tFar)
return;
if (tFar < 0)
return;
}
//assert(nearDim != -1);
//assert(farDim != -1);
if (tNear > 1.0f)
return;
neV3 contact = sensorA.pos + tNear * sensorA.dir;
neV3 sensorEnd = sensorA.pos + sensorA.dir;
f32 depth = (sensorEnd - contact).Length();
sensorA.depth = depth;
f32 factor = (sensorA.dir[nearDim] >= 0) ? -1.0f : 1.0f;
sensorA.normal = transB.rot[nearDim] * factor;
sensorA.contactPoint = contact;
sensorA.materialID = convexB.matIndex;
}
else if (convexB.type == TConvex::TERRAIN)
{
neSimpleArray<s32> & _triIndex = *convexB.as.terrain.triIndex;
s32 triangleCount = _triIndex.GetUsedCount();
neArray<neTriangle_> & triangleArray = *convexB.as.terrain.triangles;
for (s32 i = 0; i < triangleCount; i++)
{
s32 test = _triIndex[i];
neTriangle_ * t = &triangleArray[_triIndex[i]];
neV3 * vert[3];
neV3 edges[3];
neV3 normal;
f32 d;
vert[0] = &convexB.vertices[t->indices[0]];
vert[1] = &convexB.vertices[t->indices[1]];
vert[2] = &convexB.vertices[t->indices[2]];
edges[0] = *vert[1] - *vert[0];
edges[1] = *vert[2] - *vert[1];
edges[2] = *vert[0] - *vert[2];
normal = edges[0].Cross(edges[1]);
normal.Normalize();
d = normal.Dot(*vert[0]);
f32 nd = normal.Dot(sensorA.dir);
f32 np = normal.Dot(sensorA.pos);
f32 t1;
t1 = (d - np) / nd;
if (t1 > 1.0f || t1 < 0.0f)
continue;
neV3 contactPoint = sensorA.pos + sensorA.dir * t1;
if (!SameSide(contactPoint, *vert[2], *vert[0], edges[0]))
continue;
if (!SameSide(contactPoint, *vert[0], *vert[1], edges[1]))
continue;
if (!SameSide(contactPoint, *vert[1], *vert[2], edges[2]))
continue;
sensorA.depth = (1.0f - t1) * sensorA.length;
if (nd > 0.0)
sensorA.normal = normal * -1.0f;
else
sensorA.normal = normal;
sensorA.contactPoint = contactPoint;
sensorA.materialID = t->materialID;
}
}
else
{
// other primitives to do
}
}
neBool BoxTestParam::CylinderEdgeTest(ConvexTestResult & res, TConvex & cylinderB, neT3 & transB, s32 whichEdge)
{
neV3 diff = trans->pos - transB.pos;
neV3 dir = trans->rot[whichEdge].Cross(transB.rot[1]);
f32 len = dir.Length();
if (neIsConsiderZero(len))
return true;
dir *= (1.0f / len);
f32 dot = dir.Dot(diff);
if (dot > 0.0f)
{
dot *= -1.0f;
}
else
{
dir *= -1.0f;
}
f32 depth = dot + cylinderB.CylinderRadius();
neV3 contactPoint = trans->pos;
s32 i;
for (i = 0; i < 3; i++)
{
if (i == whichEdge)
continue;
dot = dir.Dot(radii[i]);
if (dot > 0.0f)
{
depth += dot;
contactPoint -= radii[i];
}
else
{
depth -= dot;
contactPoint += radii[i];
}
}
if (depth <= 0.0f)
return false;
ConvexTestResult cr;
cr.edgeA[0] = contactPoint + radii[whichEdge];
cr.edgeA[1] = contactPoint - radii[whichEdge];
cr.edgeB[0] = transB.pos + transB.rot[1] * cylinderB.CylinderHalfHeight();
cr.edgeB[1] = transB.pos - transB.rot[1] * cylinderB.CylinderHalfHeight();
f32 au, bu;
// A is the box, B is the cylinder
cr.ComputerEdgeContactPoint2(au, bu);
if (cr.depth >= res.depth)
return true;
if (cr.valid)
{
depth = cylinderB.CylinderRadius() - cr.depth;
if (depth <= 0.0f)
return false;
if (depth >= res.depth)
return true;;
res.valid = true;
res.contactNormal = dir;
res.contactA = cr.contactA;
res.contactB = cr.contactB + res.contactNormal * cylinderB.CylinderRadius();
res.depth = depth;
}
else
{
// A is the box, B is the cylinder
if (au > 0.0 && au < 1.0f)
{
// box edge and cylinder end
neV3 cylinderVert;
if (bu <= 0.0f)
{
cylinderVert = cr.edgeB[0];
}
else
{
cylinderVert = cr.edgeB[1];
}
neV3 project;
f32 dist = cylinderVert.GetDistanceFromLine2(project, cr.edgeA[0], cr.edgeA[1]);
f32 depth = cylinderB.CylinderRadius() - dist;
if (depth <= 0.0f)
return true;
if (depth >= res.depth)
return true;
res.depth = depth;
res.valid = true;
res.contactNormal = project - cylinderVert;
res.contactNormal.Normalize();
res.contactA = project;
res.contactB = cylinderVert + res.contactNormal * cylinderB.CylinderRadius();
}
else
{
neV3 boxVert;
if (au <= 0.0f)
{
boxVert = cr.edgeA[0];
}
else // au >= 1.0f
{
boxVert = cr.edgeA[1];
}
if (bu > 0.0f && bu < 1.0f)
{
// boxVert and cylinder edge
neV3 project;
f32 depth = boxVert.GetDistanceFromLine2(project, cr.edgeB[0], cr.edgeB[1]);
depth = cylinderB.CylinderRadius() - depth;
if (depth <= 0.0f)
return true;
if (depth >= res.depth)
return true;
res.depth = depth;
res.valid = true;
res.contactA = boxVert;
res.contactNormal = boxVert - project;
res.contactNormal.Normalize();
res.contactB = project + res.contactNormal * cylinderB.CylinderRadius();
}
else
{
// box vert and cylinder end
neV3 cylinderVert;
if (bu <= 0.0f)
{
cylinderVert = cr.edgeB[0];
}
else
{
cylinderVert = cr.edgeB[1];
}
neV3 diff = boxVert - cylinderVert;
f32 depth = diff.Dot(diff);
if (depth >= cylinderB.CylinderRadiusSq())
return true;
depth = sqrtf(depth);
depth = cylinderB.CylinderRadius() - depth;
if (depth >= res.depth)
return true;
res.depth = depth;
res.valid = true;
res.contactNormal = diff;
res.contactNormal.Normalize();
res.contactA = boxVert;
res.contactB = cylinderVert + res.contactNormal * cylinderB.CylinderRadius();
}
}
}
return true;
}
neBool BoxTestParam::MeasureEdgePeneration(ConvexTestResult & result, BoxTestParam & otherBox, s32 dim1, s32 dim2)
{
neV3 contactA = trans->pos;
neV3 contactB = otherBox.trans->pos;
neV3 contactNormal = trans->rot[dim1].Cross(otherBox.trans->rot[dim2]);
f32 len = contactNormal.Length();
if (neIsConsiderZero(len))
return true;
contactNormal *= (1.0f / len);
neV3 me2OtherBox = otherBox.trans->pos - trans->pos;
f32 penetrated = me2OtherBox.Dot(contactNormal);
bool reverse = false;
if (penetrated < 0.0f)
{
contactNormal = contactNormal * -1.0f;
reverse = true;
}
else
penetrated = penetrated * -1.0f;
f32 progression[4];
s32 otherAxisA1 = (dim1 + 1) % 3;
s32 otherAxisA2 = (dim1 + 2) % 3;
s32 otherAxisB1 = (dim2 + 1) % 3;
s32 otherAxisB2 = (dim2 + 2) % 3;
progression[0] = radii[otherAxisA1].Dot(contactNormal);
progression[1] = radii[otherAxisA2].Dot(contactNormal);
progression[2] = otherBox.radii[otherAxisB1].Dot(contactNormal);
progression[3] = otherBox.radii[otherAxisB2].Dot(contactNormal);
f32 sign[4];
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;
sign[3] = progression[3] > 0.0f ? 1.0f: -1.0f;
penetrated += (progression[0] * sign[0]);
penetrated += (progression[1] * sign[1]);
penetrated += (progression[2] * sign[2]);
penetrated += (progression[3] * sign[3]);
contactA += (radii[otherAxisA1] * sign[0]);
contactA += (radii[otherAxisA2] * sign[1]);
contactB -= (otherBox.radii[otherAxisB1] * sign[2]);
contactB -= (otherBox.radii[otherAxisB2] * sign[3]);
if(penetrated <= 0.0f)
return false;
if (penetrated < result.depth)
{
result.depth = penetrated;
result.valid = true;
result.edgeA[0] = contactA + (radii[dim1]);
result.edgeA[1] = contactA - (radii[dim1]);
result.edgeB[0] = contactB + (otherBox.radii[dim2]);
result.edgeB[1] = contactB - (otherBox.radii[dim2]);
result.contactA = contactA;
result.contactB = contactB;
/*
if (reverse)
result.contactX = trans->rot[dim1];
else
result.contactX = trans->rot[dim1];
result.contactY = contactNormal.Cross(result.contactX);
*/ result.contactNormal = contactNormal;
}
return true;
}
bool BoxTestParam::MeasureVertexFacePeneration(ConvexTestResult & result, BoxTestParam & otherBox, s32 whichFace)
{
neV3 me2otherBox;
me2otherBox = otherBox.trans->pos - trans->pos;
neV3 direction;
direction = trans->rot[whichFace];
neV3 contactPoint;
contactPoint = otherBox.trans->pos;
f32 penetrated;
bool reverse = false;
if ((penetrated = me2otherBox.Dot(direction)) < 0.0f)
{
direction = direction * -1.0f;
reverse = true;
}
else
{
penetrated = penetrated * -1.0f;
}
penetrated += convex->as.box.boxSize[whichFace];
neV3 progression;
progression = direction * otherBox.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 -= (otherBox.radii[0] * sign[0]);
contactPoint -= (otherBox.radii[1] * sign[1]);
contactPoint -= (otherBox.radii[2] * sign[2]);
if (penetrated <= 0.0f)
return false;
if ((penetrated + 0.0001f) < result.depth)
{
result.depth = penetrated;
result.contactA = contactPoint; // contactPoint is vertex of otherBox
result.contactB = contactPoint + direction * penetrated;
result.valid = true;
result.contactNormal = direction;
}
else if (neIsConsiderZero(penetrated - result.depth))
{
s32 otherAxis1 = neNextDim1[whichFace];
s32 otherAxis2 = neNextDim2[whichFace];
//check to see if this one fall into the faces
neV3 sub = contactPoint - trans->pos;
f32 dot = neAbs(sub.Dot(trans->rot[otherAxis1]));
if (dot > (convex->as.box.boxSize[otherAxis1] * 1.001f))
return true;// not false ???? no it is true!!!
dot = neAbs(sub.Dot(trans->rot[otherAxis2]));
if (dot > (convex->as.box.boxSize[otherAxis2] * 1.001f))
return true;// not false ???? no it is true!!!
result.depth = penetrated;
result.contactA = contactPoint;
result.contactB = contactPoint + direction * penetrated;
result.valid = true;
result.contactNormal = direction;
}
return true;
}
neBool SearchResult::SearchEETri(s32 flag, s32 aIndex, s32 bIndex, neBool & assigned)
{
assigned = false;
gEdgeStack.Init();
neByte edgeIndex;
if (flag == 0) //fv
{
// face of convex A
// vertex of triangle B
for (s32 i = 0; i < objA.mesh.numNeighbour; i++) // for each edge neighbour of Face aIndex
{
int j = 0;
while ((edgeIndex = objB.mesh.VertGetEdgeNeighbour(bIndex, j)) != 0xff)
{
gEdgeStack.Push(objA.mesh.FaceGetEdgeNeighbour(aIndex, i),
objB.mesh.VertGetEdgeNeighbour(bIndex, j));
j++;
}
}
}
else //vf
{
//vertex of convex A
//face of triangle B
s32 i = 0;
//for each edge neighbour incident to Vertex aIndex
while ((edgeIndex = objA.mesh.VertGetEdgeNeighbour(aIndex, i)) != 0xff)
{
for (s32 j = 0; j < objB.mesh.numNeighbour; j++)
{
gEdgeStack.Push(objA.mesh.VertGetEdgeNeighbour(aIndex, i),
objB.mesh.FaceGetEdgeNeighbour(bIndex, j));
}
i++;
}
}
while (!gEdgeStack.IsEmpty())
{
_num_edge_test++;
s32 edgeP, edgeQ;
gEdgeStack.Pop(edgeP, edgeQ);
// does the edge form a face
neV3 a = objA.GetWorldNormalByEdge1(edgeP);
neV3 b = objA.GetWorldNormalByEdge2(edgeP);
neV3 c = objB.GetWorldNormalByEdge1(edgeQ) * -1.0f;
neV3 d = objB.GetWorldNormalByEdge2(edgeQ) * -1.0f;
c += (TriEdgeDir[edgeQ] * 0.01f);
d += (TriEdgeDir[edgeQ] * 0.01f);
c.Normalize();
d.Normalize();
f32 cba = Determinant(c,b,a);
f32 dba = Determinant(d,b,a);
f32 prod0 = cba * dba;
if (prod0 >= -1.0e-6f)
{
continue;
}
f32 adc = Determinant(a,d,c);
f32 bdc = Determinant(b,d,c);
f32 prod1 = adc * bdc;
if (prod1 >= -1.0e-6f)
{
continue;
}
f32 prod2 = cba * bdc;
if (prod2 <= 1.0e-6f)
{
continue;
}
neV3 ai, bi;
neV3 naj, nbj;
objA.GetWorldEdgeVerts(edgeP, ai, bi);
objB.GetWorldEdgeVerts(edgeQ, naj, nbj);
naj *= -1.0f; nbj *= -1.0f;
neV3 ainaj = ai + naj;
neV3 ainbj = ai + nbj;
neV3 binaj = bi + naj;
//neV3 binbj = bi + nbj;
neV3 diff1 = ainaj - ainbj;
neV3 diff2 = ainaj - binaj ;
Face testFace;
testFace.normal = diff1.Cross(diff2);
f32 len = testFace.normal.Length();
if (neIsConsiderZero(len))
{
continue;
}
testFace.normal *= (1.0f / len);
testFace.k = testFace.normal.Dot(ainaj);
f32 testD = funcD(testFace);
if (testD >= 0)
return false;
if (testD <= dMax)
continue;
assigned = true;
dMax = testD;
face = testFace;
indexA = edgeP;
indexB = edgeQ;
typeA = SearchResult::EDGE;
typeB = SearchResult::EDGE;
// push
s32 i, j;
s32 vindex;
vindex = objB.mesh.EdgeGetVert1(edgeQ);
i = 0;
while ((j = objB.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
{
if (j != edgeQ)
gEdgeStack.Push(edgeP, j);
i++;
}
vindex = objB.mesh.EdgeGetVert2(edgeQ);
i = 0;
while ((j = objB.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
{
if (j != edgeQ)
gEdgeStack.Push(edgeP, j);
i++;
}
vindex = objA.mesh.EdgeGetVert1(edgeP);
i = 0;
while((j = objA.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
{
if (j != edgeP)
gEdgeStack.Push(j, edgeQ);
i++;
}
vindex = objA.mesh.EdgeGetVert2(edgeP);
//for (i = 0; i < objA.mesh.VertGetNumEdgeNeighbour(vindex); i++)
i = 0;
while ((j = objA.mesh.VertGetEdgeNeighbour(vindex, i)) != 0xff)
{
if (j != edgeP)
gEdgeStack.Push(j, edgeQ);
i++;
}
}
return true;
}
neBool IntersectAABBTriangle(neTriangleTree * tree, const neV3 & minBound, const neV3 & maxBound, const neTriangle & triangle)
{
neCollision box;
neCollision tri;
// neT3 boxt3;
neT3 trit3;
trit3.SetIdentity();
tri.obb.SetTriangle(triangle.indices[0], triangle.indices[1], triangle.indices[2], tree->vertices);
tri.obb.SetTransform(trit3);
tri.convexCount = 1;
tri.convex = &tri.obb;
/*
if (triangle.indices[0] == 1312 &&
triangle.indices[1] == 1363 &&
triangle.indices[2] == 1364)
{
ASSERT(0);
}
*/
neV3 edge0 = tree->vertices[triangle.indices[1]] - tree->vertices[triangle.indices[0]];
neV3 edge1 = tree->vertices[triangle.indices[2]] - tree->vertices[triangle.indices[1]];
neV3 normal = edge0.Cross(edge1);
normal.Normalize();
f32 xfactor = 0.0f;
f32 yfactor = 0.0f;
f32 zfactor = 0.0f;
if (neIsConsiderZero(neAbs(normal[0]) - 1.0f))
{
xfactor = 0.01f;
}
else if (neIsConsiderZero(neAbs(normal[1]) - 1.0f))
{
yfactor = 0.01f;
}
else if (neIsConsiderZero(neAbs(normal[2]) - 1.0f))
{
zfactor = 0.01f;
}
float boxpos[3];
float boxhalfsize[3];
float triverts[3][3];
int i;
boxhalfsize[0] = ((maxBound[0] - minBound[0]) * 0.5f) + xfactor;
boxhalfsize[1] = ((maxBound[1] - minBound[1]) * 0.5f) + yfactor;
boxhalfsize[2] = ((maxBound[2] - minBound[2]) * 0.5f) + zfactor;
for (i = 0; i < 3; i++)
boxpos[i] = (maxBound[i] + minBound[i]) * 0.5f;
for (i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
triverts[i][j] = tree->vertices[triangle.indices[i]][j];
int ret = _triBoxOverlap_(boxpos, boxhalfsize, triverts);
return ret;
/*
boxt3.SetIdentity();
box.obb.SetBoxSize((maxBound[0] - minBound[0]) + xfactor,
(maxBound[1] - minBound[1]) + yfactor,
(maxBound[2] - minBound[2]) + zfactor);
box.obb.SetTransform(boxt3);
box.convexCount = 1;
box.convex = &box.obb;
neCollisionResult res;
boxt3.pos = ( maxBound + minBound ) * 0.5f;
CollisionTest(res, box, boxt3, tri, trit3);
return res.penetrate;
*/
}
