public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
for (int j = 0; j < numVectors; j++)
{
float maxDot = float.MinValue;
IndexedVector3 vec = vectors[j];
IndexedVector3 vtx = IndexedVector3.Zero;
float newDot;
int numSpheres = m_localPositionArray.Count;
for (int i = 0; i < numSpheres; i++)
{
IndexedVector3 pos = m_localPositionArray[i];
float rad = m_radiArray[i];
vtx = (pos) + vec * m_localScaling * (rad) - vec * GetMargin();
newDot = IndexedVector3.Dot(vec, vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supportVerticesOut[j] = new IndexedVector4(vtx, 0);
}
}
}
}
void PushContact(ref IndexedVector3 point, ref IndexedVector4 normal,
float depth, int feature1, int feature2)
{
IndexedVector3 temp = new IndexedVector3(normal.X, normal.Y, normal.Z);
Add(new GIM_CONTACT(ref point, ref temp, depth, feature1, feature2));
}
public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
int i;
IndexedVector3 vtx;
float newDot = 0f;
for (i = 0; i < numVectors; i++)
{
IndexedVector4 temp = supportVerticesOut[i];
temp.W = -MathUtil.BT_LARGE_FLOAT;
supportVerticesOut[i] = temp;
}
for (int j = 0; j < numVectors; j++)
{
IndexedVector3 vec = vectors[j];
for (i = 0; i < GetNumVertices(); i++)
{
GetVertex(i, out vtx);
newDot = IndexedVector3.Dot(vec, vtx);
if (newDot > supportVerticesOut[j].W)
{
supportVerticesOut[j] = new IndexedVector4(vtx, newDot);
}
}
}
}
public void MergePoints(ref IndexedVector4 plane, float margin, IndexedVector3[] points, int point_count)
{
m_point_count = 0;
m_penetration_depth = -1000.0f;
int _k;
for (_k = 0; _k < point_count; _k++)
{
float _dist = -ClipPolygon.DistancePointPlane(ref plane, ref points[_k]) + margin;
if (_dist >= 0.0f)
{
if (_dist > m_penetration_depth)
{
m_penetration_depth = _dist;
point_indices[0] = _k;
m_point_count = 1;
}
else if ((_dist + MathUtil.SIMD_EPSILON) >= m_penetration_depth)
{
point_indices[m_point_count] = _k;
m_point_count++;
}
}
}
for (_k = 0; _k < m_point_count; _k++)
{
m_points[_k] = points[point_indices[_k]];
}
}
public virtual void RecalcLocalAabb()
{
m_isLocalAabbValid = true;
#if true
//fixme - make a static list.
IndexedVector4[] _supporting = new IndexedVector4[6];
BatchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
for (int i = 0; i < 3; ++i)
{
IndexedVector3 s0 = new IndexedVector3(_supporting[i]);
IndexedVector3 s1 = new IndexedVector3(_supporting[i + 3]);
m_localAabbMax[i] = s0[i] + m_collisionMargin;
m_localAabbMin[i] = s1[i] - m_collisionMargin;
}
#else
for (int i = 0; i < 3; i++)
{
btVector3 vec(float(0.), float(0.), float(0.));
vec[i] = float(1.);
btVector3 tmp = localGetSupportingVertex(vec);
m_localAabbMax[i] = tmp[i] + m_collisionMargin;
vec[i] = float(-1.);
tmp = localGetSupportingVertex(vec);
m_localAabbMin[i] = tmp[i] - m_collisionMargin;
}
#endif
}
public void BuildTriPlane(out IndexedVector4 plane)
{
IndexedVector3 normal = IndexedVector3.Cross(m_vertices1[1] - m_vertices1[0], m_vertices1[2] - m_vertices1[0]);
normal.Normalize();
plane = new IndexedVector4(normal, IndexedVector3.Dot(m_vertices1[0], normal));
}
public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
float newDot;
//use 'w' component of supportVerticesOut?
{
for (int i = 0; i < numVectors; i++)
{
IndexedVector4 temp = supportVerticesOut[i];
temp.W = -MathUtil.BT_LARGE_FLOAT;
supportVerticesOut[i] = temp;
}
}
for (int i = 0; i < m_unscaledPoints.Count; i++)
{
IndexedVector3 vtx = GetScaledPoint(i);
for (int j = 0; j < numVectors; j++)
{
IndexedVector3 vec = vectors[j];
newDot = IndexedVector3.Dot(vec, vtx);
if (newDot > supportVerticesOut[j].W)
{
//WARNING: don't swap next lines, the w component would get overwritten!
supportVerticesOut[j] = new IndexedVector4(vtx, newDot);
}
}
}
}
public static void bt_edge_plane(ref IndexedVector3 e1, ref IndexedVector3 e2, ref IndexedVector3 normal, out IndexedVector4 plane)
{
IndexedVector3 planenormal = (e2 - e1).Cross(ref normal);
planenormal.Normalize();
plane = new IndexedVector4(planenormal, e2.Dot(ref planenormal));
}
public static float ProjectOrigin(ref IndexedVector3 a,
ref IndexedVector3 b,
ref IndexedVector3 c,
ref IndexedVector3 d,
ref IndexedVector4 w, ref uint m)
{
//uint[] imd3 ={1,2,0};
//IndexedVector3[] vt = {a,b,c,d};
//IndexedVector3[] dl= {a-d,b-d,c-d};
Debug.Assert(inhere1 == false);
inhere1 = true;
vta[0] = a; vta[1] = b; vta[2] = c; vta[3] = d;
dla[0] = a - d; dla[1] = b - d; dla[2] = c - d;
float vl = Det(dl[0], dl[1], dl[2]);
bool ng = (vl * IndexedVector3.Dot(a, IndexedVector3.Cross(b - c, a - b))) <= 0;
if (ng && (Math.Abs(vl) > GjkEpaSolver2.GJK_SIMPLEX4_EPS))
{
float mindist = -1;
IndexedVector4 subw = new IndexedVector4();
uint subm = 0;
for (int i = 0; i < 3; ++i)
{
uint j = imd3[i];
float s = vl * IndexedVector3.Dot(d, IndexedVector3.Cross(dl[i], dl[j]));
if (s > 0)
{
float subd = GJK.ProjectOrigin(ref vt[i], ref vt[j], ref d, ref subw, ref subm);
if ((mindist < 0) || (subd < mindist))
{
mindist = subd;
m = (uint)((((subm & 1) != 0)?1 << i:0) +
(((subm & 2) != 0)?1 << (int)j:0) +
(((subm & 4) != 0)?8:0));
w[(int)i] = subw.X;
w[(int)j] = subw.Y;
w[(int)imd3[j]] = 0f;
w.W = subw.Z;
}
}
}
if (mindist < 0)
{
mindist = 0;
m = 15;
w.X = Det(c, b, d) / vl;
w.Y = Det(a, c, d) / vl;
w.Z = Det(b, a, d) / vl;
w.W = 1 - (w.X + w.Y + w.Z);
}
inhere1 = false;
return(mindist);
}
inhere1 = false;
return(-1);
}
//! Calcs the plane which is paralele to the edge and perpendicular to the triangle plane
/*!
* \pre this triangle must have its plane calculated.
*/
public void GetEdgePlane(int edge_index, out IndexedVector4 plane)
{
IndexedVector3 e0 = m_vertices[edge_index];
IndexedVector3 e1 = m_vertices[(edge_index + 1) % 3];
IndexedVector3 planeNormal = new IndexedVector3(m_plane.X, m_plane.Y, m_plane.Z);
GeometeryOperations.bt_edge_plane(ref e0, ref e1, ref planeNormal, out plane);
}
public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
for (int i = 0; i < numVectors; i++)
{
IndexedVector3 vec = vectors[i];
supportVerticesOut[i] = new IndexedVector4(ConeLocalSupport(ref vec), 0);
}
}
public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
///@todo: could make recursive use of batching. probably this shape is not used frequently.
for (int i = 0; i < numVectors; i++)
{
IndexedVector3 temp = vectors[i];
supportVerticesOut[i] = new IndexedVector4(LocalGetSupportingVertexWithoutMargin(ref temp), 0f);
}
}
public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
IndexedVector3 halfExtents = GetHalfExtentsWithoutMargin();
for (int i = 0; i < numVectors; i++)
{
supportVerticesOut[i] = new IndexedVector4(CylinderLocalSupportZ(halfExtents, vectors[i]), 0);
}
}
//! Clips a polygon by a plane
/*!
*\param clipped must be an array of 16 points.
*\return The count of the clipped counts
*/
public static int PlaneClipTriangle(
ref IndexedVector4 plane,
ref IndexedVector3 point0,
ref IndexedVector3 point1,
ref IndexedVector3 point2,
IndexedVector3[] clipped // an allocated array of 16 points at least
)
{
int clipped_count = 0;
//clip first point0
float firstdist = DistancePointPlane(ref plane, ref point0);
if (!(firstdist > MathUtil.SIMD_EPSILON))
{
clipped[clipped_count] = point0;
clipped_count++;
}
// point 1
float olddist = firstdist;
float dist = DistancePointPlane(ref plane, ref point1);
PlaneClipPolygonCollect(
ref point0, ref point1,
olddist,
dist,
clipped,
ref clipped_count);
olddist = dist;
// point 2
dist = DistancePointPlane(ref plane, ref point2);
PlaneClipPolygonCollect(
ref point1, ref point2,
olddist,
dist,
clipped,
ref clipped_count);
olddist = dist;
//RETURN TO FIRST point0
PlaneClipPolygonCollect(
ref point2, ref point0,
olddist,
firstdist,
clipped,
ref clipped_count);
return(clipped_count);
}
public static void GetPlaneEquationTransformed(StaticPlaneShape plane, ref IndexedMatrix trans, out IndexedVector4 equation)
{
equation = new IndexedVector4();
IndexedVector3 planeNormal = plane.GetPlaneNormal();
equation.X = trans._basis.GetRow(0).Dot(ref planeNormal);
equation.Y = trans._basis.GetRow(1).Dot(ref planeNormal);
equation.Z = trans._basis.GetRow(2).Dot(ref planeNormal);
equation.W = trans._origin.Dot(ref planeNormal) + plane.GetPlaneConstant();
}
public void CopyFrom(GIM_TRIANGLE_CONTACT other)
{
m_penetration_depth = other.m_penetration_depth;
m_separating_normal = other.m_separating_normal;
m_point_count = other.m_point_count;
int i = m_point_count;
while (i-- != 0)
{
m_points[i] = other.m_points[i];
}
}
public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
IndexedVector3 halfExtents = GetHalfExtentsWithoutMargin();
for (int i = 0; i < numVectors; i++)
{
IndexedVector3 vec = vectors[i];
supportVerticesOut[i] = new IndexedVector4(MathUtil.FSel(vec.X, halfExtents.X, -halfExtents.X),
MathUtil.FSel(vec.Y, halfExtents.Y, -halfExtents.Y),
MathUtil.FSel(vec.Z, halfExtents.Z, -halfExtents.Z), 0f);
}
}
public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
for (int i = 0; i < numVectors; i++)
{
IndexedVector3 dir = vectors[i];
IndexedVector3 dots = new IndexedVector3(
dir.Dot(ref m_vertices1[0]),
dir.Dot(ref m_vertices1[1]),
dir.Dot(ref m_vertices1[2]));
supportVerticesOut[i] = new IndexedVector4(m_vertices1[MathUtil.MaxAxis(ref dots)], 0);
}
}
public static float ProjectOrigin(ref IndexedVector3 a,
ref IndexedVector3 b,
ref IndexedVector3 c,
ref IndexedVector4 w, ref uint m)
{
Debug.Assert(inhere2 == false);
inhere2 = true;
vt[0] = a; vt[1] = b; vt[2] = c;
dl[0] = a - b; dl[1] = b - c; dl[2] = c - a;
IndexedVector3 n = IndexedVector3.Cross(dl[0], dl[1]);
float l = n.LengthSquared();
if (l > GjkEpaSolver2.GJK_SIMPLEX3_EPS)
{
float mindist = -1f;
IndexedVector4 subw = new IndexedVector4();
uint subm = 0;
for (int i = 0; i < 3; ++i)
{
if (IndexedVector3.Dot(vt[i], IndexedVector3.Cross(dl[i], n)) > 0)
{
uint j = imd3[i];
float subd = GJK.ProjectOrigin(ref vt[i], ref vt[j], ref subw, ref subm);
if ((mindist < 0) || (subd < mindist))
{
mindist = subd;
m = (uint)((((subm & 1) != 0)?1 << i:0) + (((subm & 2) != 0)?1 << (int)j:0));
w[(int)i] = subw.X;
w[(int)j] = subw.Y;
w[(int)imd3[j]] = 0f;
}
}
}
if (mindist < 0)
{
float d = IndexedVector3.Dot(ref a, ref n);
float s = (float)Math.Sqrt(l);
IndexedVector3 p = n * (d / l);
mindist = p.LengthSquared();
m = 7;
w.X = (IndexedVector3.Cross(dl[1], b - p)).Length() / s;
w.Y = (IndexedVector3.Cross(dl[2], c - p)).Length() / s;
w.Z = 1 - (w.X + w.Y);
}
inhere2 = false;
return(mindist);
}
inhere2 = false;
return(-1);
}
public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
for (int j = 0; j < numVectors; j++)
{
IndexedVector3 vec = vectors[j];
LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(ref vec);
IndexedVector3 aabbMax = MathUtil.MAX_VECTOR;
IndexedVector3 aabbMin = MathUtil.MIN_VECTOR;
m_stridingMesh.InternalProcessAllTriangles(supportCallback, ref aabbMin, ref aabbMax);
supportVerticesOut[j] = new IndexedVector4(supportCallback.GetSupportVertexLocal(), 0);
}
}
//! Clips a polygon by a plane
/*!
*\return The count of the clipped counts
*/
public static int PlaneClipPolygon(
ref IndexedVector4 plane,
IndexedVector3[] polygon_points,
int polygon_point_count,
IndexedVector3[] clipped)
{
int clipped_count = 0;
//IndexedVector3[] rawPoints = polygon_points.GetRawArray();
//clip first point
float firstdist = DistancePointPlane(ref plane, ref polygon_points[0]);;
if (!(firstdist > MathUtil.SIMD_EPSILON))
{
clipped[clipped_count] = polygon_points[0];
clipped_count++;
}
float olddist = firstdist;
for (int i = 1; i < polygon_point_count; i++)
{
float dist = DistancePointPlane(ref plane, ref polygon_points[i]);
PlaneClipPolygonCollect(
ref polygon_points[i - 1], ref polygon_points[i],
olddist,
dist,
clipped,
ref clipped_count);
olddist = dist;
}
//RETURN TO FIRST point
PlaneClipPolygonCollect(
ref polygon_points[polygon_point_count - 1], ref polygon_points[0],
olddist,
firstdist,
clipped,
ref clipped_count);
return(clipped_count);
}
// seems silly to duplicate this stuff when it's in xna plane, but a cleanup will wait
// till it's 'working'
public BT_PLANE_INTERSECTION_TYPE PlaneClassify(ref IndexedVector4 plane)
{
float _fmin, _fmax;
ProjectionInterval(ref plane, out _fmin, out _fmax);
if (plane.W > _fmax + BoxCollision.BOX_PLANE_EPSILON)
{
return(BT_PLANE_INTERSECTION_TYPE.BT_CONST_BACK_PLANE); // 0
}
if (plane.W + BoxCollision.BOX_PLANE_EPSILON >= _fmin)
{
return(BT_PLANE_INTERSECTION_TYPE.BT_CONST_COLLIDE_PLANE); //1
}
return(BT_PLANE_INTERSECTION_TYPE.BT_CONST_FRONT_PLANE); //2
}
public void RecalcLocalAabb()
{
m_isLocalAabbValid = true;
#if TRUE
IndexedVector3[] _directions = new IndexedVector3[6];
_directions[0] = new IndexedVector3(1, 0, 0);
_directions[1] = new IndexedVector3(0, 1, 0);
_directions[2] = new IndexedVector3(0, 0, 1);
_directions[3] = new IndexedVector3(-1, 0, 0);
_directions[4] = new IndexedVector3(0, -1, 0);
_directions[5] = new IndexedVector3(0, 0, -1);
IndexedVector4[] _supporting = new IndexedVector4[6];
BatchedUnitVectorGetSupportingVertexWithoutMargin(_directions, _supporting, 6);
for (int i = 0; i < 3; ++i)
{
IndexedVector3 temp = new IndexedVector3(_supporting[i]);
m_localAabbMax[i] = temp[i] + m_collisionMargin;
temp = new IndexedVector3(_supporting[i + 3]);
m_localAabbMin[i] = temp[i] - m_collisionMargin;
}
int ibreak = 0;
#else
for (int i = 0; i < 3; i++)
{
IndexedVector3 vec = new IndexedVector3();
MathUtil.vectorComponent(ref vec, i, 1f);
IndexedVector3 tmp = localGetSupportingVertex(ref vec);
MathUtil.vectorComponent(ref m_localAabbMax, i, (MathUtil.vectorComponent(ref tmp, i) + m_collisionMargin));
MathUtil.vectorComponent(ref vec, i, -1f);
IndexedVector3 tmp = localGetSupportingVertex(ref vec);
MathUtil.vectorComponent(ref m_localAabbMin, i, (MathUtil.vectorComponent(ref tmp, i) - m_collisionMargin));
}
#endif
}
public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
float radius = GetRadius();
for (int j = 0; j < numVectors; j++)
{
float maxDot = float.MinValue;
IndexedVector3 vec = vectors[j];
IndexedVector3 vtx;
float newDot = 0f;
{
IndexedVector3 pos = IndexedVector3.Zero;
pos[GetUpAxis()] = GetHalfHeight();
vtx = pos + vec * (radius) - vec * GetMargin();
newDot = vec.Dot(ref vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supportVerticesOut[j] = new IndexedVector4(vtx, 0);
}
}
{
IndexedVector3 pos = IndexedVector3.Zero;
pos[GetUpAxis()] = -GetHalfHeight();
vtx = pos + vec * (radius) - vec * GetMargin();
newDot = vec.Dot(ref vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supportVerticesOut[j] = new IndexedVector4(vtx, 0);
}
}
}
}
public virtual void GetPlaneEquation(out IndexedVector4 plane, int i)
{
IndexedVector3 halfExtents = GetHalfExtentsWithoutMargin();
switch (i)
{
case 0:
plane = new IndexedVector4(IndexedVector3.Right, -halfExtents.X);
break;
case 1:
plane = new IndexedVector4(IndexedVector3.Left, -halfExtents.X);
break;
case 2:
plane = new IndexedVector4(IndexedVector3.Up, -halfExtents.Y);
break;
case 3:
plane = new IndexedVector4(IndexedVector3.Down, -halfExtents.Y);
break;
case 4:
plane = new IndexedVector4(IndexedVector3.Backward, -halfExtents.Z);
break;
case 5:
plane = new IndexedVector4(IndexedVector3.Forward, -halfExtents.Z);
break;
default:
Debug.Assert(false);
plane = new IndexedVector4();
break;
}
}
public void GetPlaneEquation(out IndexedVector4 plane, int i)
{
IndexedVector3 halfExtents = GetHalfExtentsWithoutMargin();
switch (i)
{
case 0:
plane = new IndexedVector4(1, 0, 0, -halfExtents.X);
break;
case 1:
plane = new IndexedVector4(-1, 0, 0, -halfExtents.X);
break;
case 2:
plane = new IndexedVector4(0, 1, 0, -halfExtents.Y);
break;
case 3:
plane = new IndexedVector4(0, -1, 0, -halfExtents.Y);
break;
case 4:
plane = new IndexedVector4(0, 0, 1, -halfExtents.Z);
break;
case 5:
plane = new IndexedVector4(0, 0, -1, -halfExtents.Z);
break;
default:
Debug.Assert(false);
plane = IndexedVector4.Zero;
break;
}
}
public static void GetPlaneEquation(StaticPlaneShape plane, out IndexedVector4 equation)
{
equation = new IndexedVector4(plane.GetPlaneNormal(), plane.GetPlaneConstant());
}
public static float ProjectOrigin(ref IndexedVector3 a, ref IndexedVector3 b, ref IndexedVector4 w, ref uint m)
{
IndexedVector3 d = b - a;
float l = d.LengthSquared();
if (l > GjkEpaSolver2.GJK_SIMPLEX2_EPS)
{
float t = (l > 0f?(-IndexedVector3.Dot(ref a, ref d) / l):0f);
if (t >= 1)
{
w.X = 0f;
w.Y = 1f;
m = 2;
return(b.LengthSquared());
}
else if (t <= 0)
{
w.X = 1f;
w.Y = 0f;
m = 1;
return(a.LengthSquared());
}
else
{
w.X = 1 - (w.Y = t);
m = 3;
return((a + d * t).LengthSquared());
}
}
return(-1);
}
public GJKStatus Evaluate(GjkEpaSolver2MinkowskiDiff shapearg, ref IndexedVector3 guess)
{
uint iterations = 0;
float sqdist = 0f;
float alpha = 0f;
uint clastw = 0;
/* Initialize solver */
m_free[0] = m_store[0];
m_free[1] = m_store[1];
m_free[2] = m_store[2];
m_free[3] = m_store[3];
m_nfree = 4;
m_current = 0;
m_status = GJKStatus.Valid;
m_shape = shapearg;
m_distance = 0f;
/* Initialize simplex */
m_simplices[0].rank = 0;
m_ray = guess;
float sqrl = m_ray.LengthSquared();
IndexedVector3 temp = sqrl > 0?-m_ray:new IndexedVector3(1, 0, 0);
AppendVertice(m_simplices[0], ref temp);
m_simplices[0].p[0] = 1;
m_ray = m_simplices[0].c[0].w;
sqdist = sqrl;
lastw[0] = lastw[1] = lastw[2] = lastw[3] = m_ray;
/* Loop */
do
{
uint next = 1 - m_current;
sSimplex cs = m_simplices[m_current];
sSimplex ns = m_simplices[next];
/* Check zero */
float rl = m_ray.Length();
if (rl < GjkEpaSolver2.GJK_MIN_DISTANCE)
{/* Touching or inside */
m_status = GJKStatus.Inside;
break;
}
/* Append new vertice in -'v' direction */
IndexedVector3 temp2 = -m_ray;
AppendVertice(cs, ref temp2);
IndexedVector3 w = cs.c[cs.rank - 1].w;
bool found = false;
for (int i = 0; i < 4; ++i)
{
if ((w - lastw[i]).LengthSquared() < GjkEpaSolver2.GJK_DUPLICATED_EPS)
{
found = true;
break;
}
}
if (found)
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
else
{/* Update lastw */
lastw[clastw = (clastw + 1) & 3] = w;
}
/* Check for termination */
float omega = IndexedVector3.Dot(ref m_ray, ref w) / rl;
alpha = Math.Max(omega, alpha);
if (((rl - alpha) - (GjkEpaSolver2.GJK_ACCURARY * rl)) <= 0)
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
/* Reduce simplex */
IndexedVector4 weights = new IndexedVector4();
uint mask = 0;
switch (cs.rank)
{
case 2:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref weights, ref mask);
break;
}
case 3:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref weights, ref mask);
break;
}
case 4:
{
sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, ref weights, ref mask);
break;
}
}
if (sqdist >= 0)
{/* Valid */
ns.rank = 0;
m_ray = IndexedVector3.Zero;
m_current = next;
for (uint i = 0, ni = cs.rank; i < ni; ++i)
{
if ((mask & (1 << (int)i)) != 0)
{
ns.c[ns.rank] = cs.c[i];
float weight = weights[(int)i];
ns.p[ns.rank++] = weight;
m_ray += cs.c[i].w * weight;
}
else
{
m_free[m_nfree++] = cs.c[i];
}
}
if (mask == 15)
{
m_status = GJKStatus.Inside;
}
}
else
{/* Return old simplex */
RemoveVertice(m_simplices[m_current]);
break;
}
m_status = ((++iterations) < GjkEpaSolver2.GJK_MAX_ITERATIONS) ? m_status : GJKStatus.Failed;
} while(m_status == GJKStatus.Valid);
m_simplex = m_simplices[m_current];
switch (m_status)
{
case GJKStatus.Valid:
{
m_distance = m_ray.Length();
break;
}
case GJKStatus.Inside:
{
m_distance = 0;
break;
}
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("gjk eval dist[{0}]", m_distance));
}
#endif
return(m_status);
}
public bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
ref IndexedVector3 v, ref IndexedVector3 pa, ref IndexedVector3 pb, IDebugDraw debugDraw)
{
bool check2d = convexA.IsConvex2d() && convexB.IsConvex2d();
float minProj = float.MaxValue;
IndexedVector3 minNorm = IndexedVector3.Zero;
IndexedVector3 minA = IndexedVector3.Zero, minB = IndexedVector3.Zero;
IndexedVector3 seperatingAxisInA, seperatingAxisInB;
IndexedVector3 pInA, qInB, pWorld, qWorld, w;
#if USE_BATCHED_SUPPORT
IndexedVector4[] supportVerticesABatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector4[] supportVerticesBBatch = new IndexedVector4[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInABatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
IndexedVector3[] seperatingAxisInBBatch = new IndexedVector3[NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
int numSampleDirections = NUM_UNITSPHERE_POINTS;
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[i], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[i], ref norm, ref transB._basis);
//seperatingAxisInABatch[i] = (-norm) * transA._basis;
//seperatingAxisInBBatch[i] = norm * transB._basis;
}
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transA._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm;
convexB.GetPreferredPenetrationDirection(i, out norm);
IndexedBasisMatrix.Multiply(ref norm, ref transB._basis, ref norm);
sPenetrationDirections[numSampleDirections] = norm;
IndexedVector3 negNorm = -norm;
IndexedBasisMatrix.Multiply(ref seperatingAxisInABatch[numSampleDirections], ref negNorm, ref transA._basis);
IndexedBasisMatrix.Multiply(ref seperatingAxisInBBatch[numSampleDirections], ref norm, ref transB._basis);
numSampleDirections++;
}
}
}
convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch, numSampleDirections);
convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
// shouldn't this be Y ?
norm.Z = 0;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = seperatingAxisInABatch[i];
seperatingAxisInB = seperatingAxisInBBatch[i];
pInA = new IndexedVector3(supportVerticesABatch[i].X, supportVerticesABatch[i].Y, supportVerticesABatch[i].Z);
qInB = new IndexedVector3(supportVerticesBBatch[i].X, supportVerticesBBatch[i].Y, supportVerticesBBatch[i].Z);
IndexedMatrix.Multiply(out pWorld, ref transA, ref pInA);
IndexedMatrix.Multiply(out qWorld, ref transB, ref qInB);
if (check2d)
{
// shouldn't this be Y ?
pWorld.Z = 0f;
qWorld.Z = 0f;
}
IndexedVector3.Subtract(out w, ref qWorld, ref pWorld);
float delta = IndexedVector3.Dot(ref norm, ref w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#else
int numSampleDirections = NUM_UNITSPHERE_POINTS;
{
int numPDA = convexA.GetNumPreferredPenetrationDirections();
if (numPDA > 0)
{
for (int i = 0; i < numPDA; i++)
{
IndexedVector3 norm;
convexA.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transA);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
{
int numPDB = convexB.GetNumPreferredPenetrationDirections();
if (numPDB > 0)
{
for (int i = 0; i < numPDB; i++)
{
IndexedVector3 norm = IndexedVector3.Zero;
convexB.GetPreferredPenetrationDirection(i, out norm);
norm = IndexedVector3.TransformNormal(norm, transB);
sPenetrationDirections[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
for (int i = 0; i < numSampleDirections; i++)
{
IndexedVector3 norm = sPenetrationDirections[i];
if (check2d)
{
norm.Z = 0f;
}
if (norm.LengthSquared() > 0.01f)
{
seperatingAxisInA = IndexedVector3.TransformNormal(-norm, transA);
seperatingAxisInB = IndexedVector3.TransformNormal(norm, transB);
pInA = convexA.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInA);
qInB = convexB.LocalGetSupportVertexWithoutMarginNonVirtual(ref seperatingAxisInB);
pWorld = IndexedVector3.Transform(pInA, transA);
qWorld = IndexedVector3.Transform(qInB, transB);
if (check2d)
{
pWorld.Z = 0.0f;
qWorld.Z = 0.0f;
}
w = qWorld - pWorld;
float delta = IndexedVector3.Dot(norm, w);
//find smallest delta
if (delta < minProj)
{
minProj = delta;
minNorm = norm;
minA = pWorld;
minB = qWorld;
}
}
}
#endif //USE_BATCHED_SUPPORT
//add the margins
minA += minNorm * convexA.GetMarginNonVirtual();
minB -= minNorm * convexB.GetMarginNonVirtual();
//no penetration
if (minProj < 0f)
{
return(false);
}
float extraSeparation = 0.5f;///scale dependent
minProj += extraSeparation + (convexA.GetMarginNonVirtual() + convexB.GetMarginNonVirtual());
#if DEBUG_DRAW
if (debugDraw)
{
IndexedVector3 color = new IndexedVector3(0, 1, 0);
debugDraw.drawLine(minA, minB, color);
color = new IndexedVector3(1, 1, 1);
IndexedVector3 vec = minB - minA;
float prj2 = IndexedVector3.Dot(minNorm, vec);
debugDraw.drawLine(minA, minA + (minNorm * minProj), color);
}
#endif //DEBUG_DRAW
GjkPairDetector gjkdet = BulletGlobals.GjkPairDetectorPool.Get();
gjkdet.Initialize(convexA, convexB, simplexSolver, null);
float offsetDist = minProj;
IndexedVector3 offset = minNorm * offsetDist;
ClosestPointInput input = ClosestPointInput.Default();
IndexedVector3 newOrg = transA._origin + offset;
IndexedMatrix displacedTrans = transA;
displacedTrans._origin = newOrg;
input.m_transformA = displacedTrans;
input.m_transformB = transB;
input.m_maximumDistanceSquared = float.MaxValue;
MinkowskiIntermediateResult res = new MinkowskiIntermediateResult();
gjkdet.SetCachedSeperatingAxis(-minNorm);
gjkdet.GetClosestPoints(ref input, res, debugDraw, false);
float correctedMinNorm = minProj - res.m_depth;
//the penetration depth is over-estimated, relax it
float penetration_relaxation = 1f;
minNorm *= penetration_relaxation;
if (res.m_hasResult)
{
pa = res.m_pointInWorld - minNorm * correctedMinNorm;
pb = res.m_pointInWorld;
v = minNorm;
#if DEBUG_DRAW
if (debugDraw != null)
{
IndexedVector3 color = new IndexedVector3(1, 0, 0);
debugDraw.drawLine(pa, pb, color);
}
#endif//DEBUG_DRAW
}
BulletGlobals.GjkPairDetectorPool.Free(gjkdet);
return(res.m_hasResult);
}
