public void Project(ref IndexedMatrix trans, ref IndexedVector3 dir, ref float min, ref float max)
{
#if true
min = float.MaxValue;
max = float.MinValue;
IndexedVector3 witnesPtMin;
IndexedVector3 witnesPtMax;
int numVerts = m_unscaledPoints.Count;
for(int i=0;i<numVerts;i++)
{
IndexedVector3 vtx = m_unscaledPoints[i] * m_localScaling;
IndexedVector3 pt = trans * vtx;
float dp = pt.Dot(dir);
if(dp < min)
{
min = dp;
witnesPtMin = pt;
}
if(dp > max)
{
max = dp;
witnesPtMax=pt;
}
}
#else
btVector3 localAxis = dir*trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
min = vtx1.dot(dir);
max = vtx2.dot(dir);
#endif
if(min>max)
{
float tmp = min;
min = max;
max = tmp;
}
}
public static void ClipFaceAgainstHull(ref IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ref IndexedMatrix transA, ObjectArray<IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ObjectArray<IndexedVector3> worldVertsB2 = new ObjectArray<IndexedVector3>();
ObjectArray<IndexedVector3> pVtxIn = worldVertsB1;
ObjectArray<IndexedVector3> pVtxOut = worldVertsB2;
pVtxOut.Capacity = pVtxIn.Count;
int closestFaceA = -1;
{
float dmin = float.MaxValue;
for (int face = 0; face < hullA.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
IndexedVector3 faceANormalWS = transA._basis * Normal;
float d = IndexedVector3.Dot(faceANormalWS, separatingNormal);
if (d < dmin)
{
dmin = d;
closestFaceA = face;
}
}
}
if (closestFaceA < 0)
return;
Face polyA = hullA.m_faces[closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
int numContacts = pVtxIn.Count;
int numVerticesA = polyA.m_indices.Count;
for (int e0 = 0; e0 < numVerticesA; e0++)
{
IndexedVector3 a = hullA.m_vertices[polyA.m_indices[e0]];
IndexedVector3 b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
IndexedVector3 edge0 = a - b;
IndexedVector3 WorldEdge0 = transA._basis * edge0;
IndexedVector3 worldPlaneAnormal1 = transA._basis * new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
IndexedVector3 planeNormalWS1 = -WorldEdge0.Cross(worldPlaneAnormal1);//.cross(WorldEdge0);
IndexedVector3 worldA1 = transA * a;
float planeEqWS1 = -worldA1.Dot(planeNormalWS1);
//int otherFace=0;
#if BLA1
int otherFace = polyA.m_connectedFaces[e0];
btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
#else
IndexedVector3 planeNormalWS = planeNormalWS1;
float planeEqWS=planeEqWS1;
#endif //clip face
ClipFace(pVtxIn, pVtxOut, ref planeNormalWS, planeEqWS);
//btSwap(pVtxIn,pVtxOut);
ObjectArray<IndexedVector3> temp = pVtxIn;
pVtxIn = pVtxOut;
pVtxOut = temp;
pVtxOut.Clear();
}
//#define ONLY_REPORT_DEEPEST_POINT
IndexedVector3 point;
// only keep points that are behind the witness face
{
IndexedVector3 localPlaneNormal = new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
float localPlaneEq = polyA.m_plane[3];
IndexedVector3 planeNormalWS = transA._basis * localPlaneNormal;
float planeEqWS = localPlaneEq - IndexedVector3.Dot(planeNormalWS, transA._origin);
for (int i = 0; i < pVtxIn.Count; i++)
{
float depth = IndexedVector3.Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
if (depth <= minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <= maxDist && depth >= minDist)
{
IndexedVector3 point2 = pVtxIn[i];
#if ONLY_REPORT_DEEPEST_POINT
curMaxDist = depth;
#else
#if false
if (depth<-3)
{
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
printf("likely wrong separatingNormal passed in\n");
}
#endif
resultOut.AddContactPoint(ref separatingNormal, ref point2, depth);
#endif
}
}
}
#if ONLY_REPORT_DEEPEST_POINT
if (curMaxDist<maxDist)
{
resultOut.AddContactPoint(ref separatingNormal,ref point,curMaxDist);
}
#endif //ONLY_REPORT_DEEPEST_POINT
}
public override void GetAabbSlow(ref IndexedMatrix t, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
#if true
IndexedVector3[] _directions = new IndexedVector3[]
{
new IndexedVector3( 1.0f, 0.0f, 0.0f),
new IndexedVector3( 0.0f, 1.0f, 0.0f),
new IndexedVector3( 0.0f, 0.0f, 1.0f),
new IndexedVector3( -1.0f, 0.0f, 0.0f),
new IndexedVector3( 0.0f, -1.0f, 0.0f),
new IndexedVector3( 0.0f, 0.0f, -1.0f)
};
IndexedVector4[] _supporting = new IndexedVector4[]
{
IndexedVector4.Zero,
IndexedVector4.Zero,
IndexedVector4.Zero,
IndexedVector4.Zero,
IndexedVector4.Zero,
IndexedVector4.Zero
};
for (int i = 0; i < 6; i++)
{
_directions[i] = _directions[i] * t._basis;
}
ObjectArray<IndexedVector4> tempSupporting = new ObjectArray<IndexedVector4>(6);
BatchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
IndexedVector3 aabbMin1 = new IndexedVector3(0,0,0),aabbMax1 = new IndexedVector3(0,0,0);
for ( int i = 0; i < 3; ++i )
{
IndexedVector3 temp = new IndexedVector3(_supporting[i].X, _supporting[i].Y, _supporting[i].Z);
aabbMax1[i] = (t *temp)[i];
temp = new IndexedVector3(_supporting[i+3].X, _supporting[i+3].Y, _supporting[i+3].Z);
aabbMin1[i] = (t * temp)[i];
}
IndexedVector3 marginVec = new IndexedVector3(GetMargin());
aabbMin = aabbMin1-marginVec;
aabbMax = aabbMax1+marginVec;
#else
float margin = getMargin();
for (int i=0;i<3;i++)
{
IndexedVector3 vec(float(0.),float(0.),float(0.));
vec[i] = float(1.);
IndexedVector3 sv = localGetSupportingVertex(vec*t.getBasis());
IndexedVector3 tmp = t(sv);
aabbMax[i] = tmp[i]+margin;
vec[i] = float(-1.);
sv = localGetSupportingVertex(vec*t.getBasis());
tmp = t(sv);
aabbMin[i] = tmp[i]-margin;
}
#endif
}