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 IndexedVector4(ref IndexedVector4 v)
{
X = v.X;
Y = v.Y;
Z = v.Z;
W = v.W;
}
public IndexedVector4(ref IndexedVector4 v)
{
X = v.X;
Y = v.Y;
Z = v.Z;
W = v.W;
}
//notice that the vectors should be unit length
public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
for (int i=0;i<numVectors;i++)
{
supportVerticesOut[i] = IndexedVector4.Zero;
}
}
public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors)
{
m_childConvexShape.BatchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors);
for (int i=0;i<numVectors;i++)
{
supportVerticesOut[i] = (supportVerticesOut[i] * m_uniformScalingFactor); ;
}
}
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 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);
}
}
//! 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;
}
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
}
public static IndexedVector3 PlaneLineIntersection(ref IndexedVector4 plane, ref IndexedVector3 p0, ref IndexedVector3 p1)
{
// returns the point where the line p0-p1 intersects the IndexedVector4 n&
IndexedVector3 dif = p1 - p0;
float dn = IndexedVector3.Dot(plane.ToVector3(), dif);
float t = -(plane.W + IndexedVector3.Dot(plane.ToVector3(), p0)) / dn;
return p0 + (dif * t);
}
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);
}
}
public IndexedVector3(IndexedVector4 v)
{
X = v.X;
Y = v.Y;
Z = v.Z;
}
public static void GetPlaneEquation(StaticPlaneShape plane,out IndexedVector4 equation)
{
equation = new IndexedVector4(plane.GetPlaneNormal(),plane.GetPlaneConstant());
}
//notice that the vectors should be unit length public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors);
public static PlaneIntersectType SplitTest(ConvexH convex, ref IndexedVector4 IndexedVector4)
{
PlaneIntersectType flag = PlaneIntersectType.COPLANAR;
for (int i = 0; i < convex.vertices.Count; i++)
{
IndexedVector3 vtx = convex.vertices[i];
flag |= PlaneTest(ref IndexedVector4, ref vtx);
}
return flag;
}
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));
}
//! 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)
{
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 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;
}
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 static IndexedVector3 ThreePlaneIntersection(IndexedVector4 p0, IndexedVector4 p1, IndexedVector4 p2)
{
IndexedVector3 N1 = p0.ToVector3();
IndexedVector3 N2 = p1.ToVector3();
IndexedVector3 N3 = p2.ToVector3();
IndexedVector3 n2n3 = IndexedVector3.Cross(N2, N3);
IndexedVector3 n3n1 = IndexedVector3.Cross(N3, N1);
IndexedVector3 n1n2 = IndexedVector3.Cross(N1, N2);
float quotient = IndexedVector3.Dot(N1, n2n3);
Debug.Assert(Math.Abs(quotient) > 0.000001f);
quotient = -1.0f / quotient;
n2n3 *= p0.W;
n3n1 *= p1.W;
n1n2 *= p2.W;
IndexedVector3 potentialVertex = n2n3;
potentialVertex += n3n1;
potentialVertex += n1n2;
potentialVertex *= quotient;
IndexedVector3 result = potentialVertex;
return result;
}
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 PlaneIntersectType PlaneTest(ref IndexedVector4 p, ref IndexedVector3 v)
{
float planetestepsilon = 0.0001f;
float a = IndexedVector3.Dot(v, p.ToVector3()) + p.W;
PlaneIntersectType flag = (a > planetestepsilon) ? PlaneIntersectType.OVER : ((a < -planetestepsilon) ? PlaneIntersectType.UNDER : PlaneIntersectType.COPLANAR);
return flag;
}
public IndexedVector3(IndexedVector4 v)
{
X = v.X;
Y = v.Y;
Z = v.Z;
}
public static float DistanceBetweenLines(ref IndexedVector3 ustart, ref IndexedVector3 udir, ref IndexedVector3 vstart, ref IndexedVector3 vdir, ref IndexedVector3? upoint, ref IndexedVector3? vpoint)
{
IndexedVector3 cp = IndexedVector3.Cross(udir, vdir);
cp.Normalize();
float distu = -IndexedVector3.Dot(cp, ustart);
float distv = -IndexedVector3.Dot(cp, vstart);
float dist = (float)Math.Abs(distu - distv);
if (upoint.HasValue)
{
IndexedVector4 plane = new IndexedVector4(IndexedVector3.Cross(vdir, cp).Normalized(),0);
plane.W = -IndexedVector3.Dot(plane.ToVector3(), vstart);
IndexedVector3 a = ustart + udir;
upoint = PlaneLineIntersection(ref plane, ref ustart, ref a);
}
if (vpoint.HasValue)
{
IndexedVector4 plane = new IndexedVector4(IndexedVector3.Cross(udir, cp).Normalized(), 0);
plane.W = -IndexedVector3.Dot(plane.ToVector3(), ustart);
IndexedVector3 a = vstart + vdir;
vpoint = PlaneLineIntersection(ref plane, ref vstart, ref a);
}
return dist;
}
public static IndexedVector3 PlaneProject(ref IndexedVector4 plane, ref IndexedVector3 point)
{
return point - plane.ToVector3() * (IndexedVector3.Dot(point, plane.ToVector3()) + plane.W);
}
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 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 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);
}
}