public void Abs(out IndexedVector3 result)
{
result.X = System.Math.Abs(X);
result.Y = System.Math.Abs(Y);
result.Z = System.Math.Abs(Z);
}
public static void Dot(ref IndexedVector3 a, ref IndexedVector3 b,out float r)
{
r = (a.X * b.X) + (a.Y * b.Y) + (a.Z * b.Z);
}
public static void TransformNormal(ref IndexedVector3 position, ref Matrix matrix, out IndexedVector3 vector)
{
float num3 = (((position.X * matrix.M11) + (position.Y * matrix.M21)) + (position.Z * matrix.M31));
float num2 = (((position.X * matrix.M12) + (position.Y * matrix.M22)) + (position.Z * matrix.M32));
float num = (((position.X * matrix.M13) + (position.Y * matrix.M23)) + (position.Z * matrix.M33));
vector.X = num3;
vector.Y = num2;
vector.Z = num;
}
public IndexedVector3(IndexedVector3 v)
{
X = v.X;
Y = v.Y;
Z = v.Z;
}
public static void DMULTIPLY0_331(ref IndexedVector3 A, float[] B, ref IndexedVector3 C)
{
A[0] = DDOT(B, 0, ref C, 0);
A[1] = DDOT(B, 4, ref C, 0);
A[2] = DDOT(B, 8, ref C, 0);
}
public static IndexedVector3 Transform(ref IndexedVector3 position, ref Matrix matrix)
{
IndexedVector3 vector;
float num3 = (((position.X * matrix.M11) + (position.Y * matrix.M21)) + (position.Z * matrix.M31)) + matrix.M41;
float num2 = (((position.X * matrix.M12) + (position.Y * matrix.M22)) + (position.Z * matrix.M32)) + matrix.M42;
float num = (((position.X * matrix.M13) + (position.Y * matrix.M23)) + (position.Z * matrix.M33)) + matrix.M43;
vector.X = num3;
vector.Y = num2;
vector.Z = num;
return vector;
}
private static float DDOTpq(ref IndexedVector3 a, ref IndexedVector3 b, int aOffset, int bOffset, int p, int q)
{
//return (a[0] * b[0] + (a)[p] * (b)[q] + (a)[2 * (p)] * (b)[2 * (q)]);
return (a[aOffset] * b[bOffset] + a[p + aOffset] * b[q + bOffset] + a[(2 * p) + aOffset] * b[(2 * q) + bOffset]);
}
// Work in progress to copy redo the box detector to remove un-necessary allocations
#if true
public virtual void GetClosestPoints(ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults)
{
Matrix transformA = input.m_transformA;
Matrix transformB = input.m_transformB;
if (BulletGlobals.g_streamWriter != null && debugBoxBox)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB);
}
int skip = 0;
Object contact = null;
Matrix rotateA = Matrix.Identity;
rotateA.Backward = transformA.Backward;
rotateA.Right = transformA.Right;
rotateA.Up = transformA.Up;
Matrix rotateB = Matrix.Identity;
rotateB.Backward = transformB.Backward;
rotateB.Right = transformB.Right;
rotateB.Up = transformB.Up;
IndexedVector3 normal = new IndexedVector3();
float depth = 0f;
int return_code = -1;
int maxc = 4;
IndexedVector3 translationA = new IndexedVector3(transformA.Translation);
IndexedVector3 translationB = new IndexedVector3(transformB.Translation);
Vector3 debugExtents = new Vector3(2f, 2f, 2f);
IndexedVector3 box1Margin = new IndexedVector3(2f * m_box1.GetHalfExtentsWithMargin());
IndexedVector3 box2Margin = new IndexedVector3(2f * m_box2.GetHalfExtentsWithMargin());
//Vector3 box1Margin = 2f * debugExtents;
//Vector3 box2Margin = 2f * debugExtents;
rotateA = Matrix.Transpose(rotateA);
rotateB = Matrix.Transpose(rotateB);
float[] temp1 = s_temp1;
float[] temp2 = s_temp2;
temp1[0] = rotateA.M11;
temp1[1] = rotateA.M12;
temp1[2] = rotateA.M13;
temp1[4] = rotateA.M21;
temp1[5] = rotateA.M22;
temp1[6] = rotateA.M23;
temp1[8] = rotateA.M31;
temp1[9] = rotateA.M32;
temp1[10] = rotateA.M33;
temp2[0] = rotateB.M11;
temp2[1] = rotateB.M12;
temp2[2] = rotateB.M13;
temp2[4] = rotateB.M21;
temp2[5] = rotateB.M22;
temp2[6] = rotateB.M23;
temp2[8] = rotateB.M31;
temp2[9] = rotateB.M32;
temp2[10] = rotateB.M33;
DBoxBox2(ref translationA,
temp1,
ref box1Margin,
ref translationB,
temp2,
ref box2Margin,
ref normal, ref depth, ref return_code,
maxc, contact, skip,
output);
}
private static float DDOT41(float[] a, int aOffset, ref IndexedVector3 b, int bOffset) { return DDOTpq(a, ref b, aOffset, bOffset, 4, 1); }
private static float DDOT14(ref IndexedVector3 a, int aOffset, float[] b, int bOffset) { return DDOTpq(ref a, b, aOffset, bOffset, 1, 4); }
private static float DDOT(ref IndexedVector3 a, int aOffset, ref IndexedVector3 b, int bOffset) { return DDOTpq(ref a, ref b, aOffset, bOffset, 1, 1); }
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
private static bool TST2(float expr1, float expr2, float n1, float n2, float n3, ref IndexedVector3 normalC, ref float[] normalR, int cc, ref int code, ref float s, ref bool invert_normal)
{
float s2 = System.Math.Abs(expr1) - (expr2);
if (s2 > MathUtil.SIMD_EPSILON)
{
return true;
}
float l = (float)System.Math.Sqrt((n1 * n1) + (n2 * n2) + (n3 * n3));
if (l > MathUtil.SIMD_EPSILON)
{
s2 /= l;
if (s2 * fudge_factor > s)
{
s = s2;
normalR = null;
normalC[0] = (n1) / l; normalC[1] = (n2) / l; normalC[2] = (n3) / l;
invert_normal = ((expr1) < 0);
invert_normal = ((expr1) < 0);
code = (cc);
}
}
return false;
}
private static void DLineClosestApproach(ref IndexedVector3 pa, ref IndexedVector3 ua,
ref IndexedVector3 pb, ref IndexedVector3 ub,
ref float alpha, ref float beta)
{
IndexedVector3 p = pb - pa;
float uaub = IndexedVector3.Dot(ref ua, ref ub);
float q1 = IndexedVector3.Dot(ref ua, ref p);
float q2 = -IndexedVector3.Dot(ref ub, ref p);
float d = 1 - uaub * uaub;
if (d <= 0.0001f)
{
// @@@ this needs to be made more robust
alpha = 0f;
beta = 0f;
}
else
{
d = 1f / d;
alpha = (q1 + uaub * q2) * d;
beta = (uaub * q1 + q2) * d;
}
}
public static float Dot(IndexedVector3 a, Vector3 b)
{
return Dot(ref a, ref b);
}
public static void DMULTIPLY1_331(ref IndexedVector3 A, float[] B, ref IndexedVector3 C)
{
A[0] = DDOT41(B, 0, ref C, 0);
A[1] = DDOT41(B, 1, ref C, 0);
A[2] = DDOT41(B, 2, ref C, 0);
}
public static float Dot(ref IndexedVector3 a, ref Vector3 b)
{
return (a.X * b.X) + (a.Y * b.Y) + (a.Z * b.Z);
}
private static int DBoxBox2(ref IndexedVector3 p1, float[] R1,
ref IndexedVector3 side1, ref IndexedVector3 p2,
float[] R2, ref IndexedVector3 side2,
ref IndexedVector3 normal, ref float depth, ref int return_code,
int maxc, Object contact, int skip, IDiscreteCollisionDetectorInterfaceResult output)
{
Vector3 centerDifference = Vector3.Zero, ppv = Vector3.Zero;
float[] normalR = null;
int normalROffsetResult = 0;
IndexedVector3 A = side1 * 0.5f;
IndexedVector3 B = side2 * 0.5f;
int code;
bool invert_normal;
// get vector from centers of box 1 to box 2, relative to box 1
IndexedVector3 p = p2 - p1;
IndexedVector3 pp = new IndexedVector3();
DMULTIPLY1_331(ref pp, R1, ref p); // get pp = p relative to body 1
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
float R11 = DDOT44(R1, 0, R2, 0);
float R12 = DDOT44(R1, 0, R2, 1);
float R13 = DDOT44(R1, 0, R2, 2);
float R21 = DDOT44(R1, 1, R2, 0);
float R22 = DDOT44(R1, 1, R2, 1);
float R23 = DDOT44(R1, 1, R2, 2);
float R31 = DDOT44(R1, 2, R2, 0);
float R32 = DDOT44(R1, 2, R2, 1);
float R33 = DDOT44(R1, 2, R2, 2);
float Q11 = System.Math.Abs(R11);
float Q12 = System.Math.Abs(R12);
float Q13 = System.Math.Abs(R13);
float Q21 = System.Math.Abs(R21);
float Q22 = System.Math.Abs(R22);
float Q23 = System.Math.Abs(R23);
float Q31 = System.Math.Abs(R31);
float Q32 = System.Math.Abs(R32);
float Q33 = System.Math.Abs(R33);
float s = -float.MaxValue;
invert_normal = false;
code = 0;
int normalROffset = 0;
// separating axis = u1,u2,u3
if (TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1, ref normalR, 0, ref normalROffset, 1, ref code, ref s, ref invert_normal)) return 0;
if (TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1, ref normalR, 1, ref normalROffset, 2, ref code, ref s, ref invert_normal)) return 0;
if (TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1, ref normalR, 2, ref normalROffset, 3, ref code, ref s, ref invert_normal)) return 0;
// separating axis = v1,v2,v3
if (TST(DDOT41(R2, 0, ref p, 0), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2, ref normalR, 0, ref normalROffset, 4, ref code, ref s, ref invert_normal)) return 0;
if (TST(DDOT41(R2, 1, ref p, 0), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2, ref normalR, 1, ref normalROffset, 5, ref code, ref s, ref invert_normal)) return 0;
if (TST(DDOT41(R2, 2, ref p, 0), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2, ref normalR, 2, ref normalROffset, 6, ref code, ref s, ref invert_normal)) return 0;
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
// separating axis = u1 x (v1,v2,v3)
//private static bool TST2(float expr1,float expr2,ref Vector3 normal, ref Vector3 normalC,int cc,ref int code)
IndexedVector3 normalC = new IndexedVector3();
// separating axis = u1 x (v1,v2,v3)
if (TST2(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, ref normalC, ref normalR, 7, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, ref normalC, ref normalR, 8, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, ref normalC, ref normalR, 9, ref code, ref s, ref invert_normal)) return 0;
// separating axis = u2 x (v1,v2,v3)
if (TST2(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, ref normalC, ref normalR, 10, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, ref normalC, ref normalR, 11, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, ref normalC, ref normalR, 12, ref code, ref s, ref invert_normal)) return 0;
// separating axis = u3 x (v1,v2,v3)
if (TST2(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, ref normalC, ref normalR, 13, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, ref normalC, ref normalR, 14, ref code, ref s, ref invert_normal)) return 0;
if (TST2(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, ref normalC, ref normalR, 15, ref code, ref s, ref invert_normal)) return 0;
if (code == 0)
{
return 0;
}
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR != null)
{
normal[0] = normalR[0 + normalROffset];
normal[1] = normalR[4 + normalROffset];
normal[2] = normalR[8 + normalROffset];
}
else
{
DMULTIPLY0_331(ref normal, R1, ref normalC);
}
if (invert_normal)
{
normal = -normal;
}
depth = -s;
// compute contact point(s)
if (code > 6)
{
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
IndexedVector3 pa1 = p1;
for (int j = 0; j < 3; j++)
{
float sign = (DDOT14(ref normal, 0, R1, j) > 0) ? 1.0f : -1.0f;
for (int i = 0; i < 3; i++)
{
pa1[i] += sign * A[j] * R1[i * 4 + j];
}
}
// find a point pb on the intersecting edge of box 2
IndexedVector3 pb1 = p2;
//for (i = 0; i < 3; i++) pb[i] = p2[i];
for (int j = 0; j < 3; j++)
{
float sign = (DDOT14(ref normal, 0, R2, j) > 0) ? -1.0f : 1.0f;
for (int i = 0; i < 3; i++)
{
pb1[i] += sign * B[j] * R2[i * 4 + j];
}
}
float alpha = 0f, beta = 0f;
IndexedVector3 ua = new IndexedVector3();
IndexedVector3 ub = new IndexedVector3();
for (int i = 0; i < 3; i++)
{
ua[i] = R1[((code) - 7) / 3 + i * 4];
}
for (int i = 0; i < 3; i++)
{
ub[i] = R2[((code) - 7) % 3 + i * 4];
}
DLineClosestApproach(ref pa1, ref ua, ref pb1, ref ub, ref alpha, ref beta);
for (int i = 0; i < 3; i++)
{
pa1[i] += ua[i] * alpha;
}
for (int i = 0; i < 3; i++)
{
pb1[i] += ub[i] * beta;
}
{
//contact[0].pos[i] = float(0.5)*(pa[i]+pb[i]);
//contact[0].depth = *depth;
#if USE_CENTER_POINT
pointInWorld = (pa + pb) * 0.5f;
output.addContactPoint(-normal,pointInWorld,-depth);
#else
Vector3 pbv = pb1.ToVector3();
output.AddContactPoint((-normal).ToVector3(), pbv, -depth);
#endif //
return_code = code;
}
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
float[] Ra;
float[] Rb;
IndexedVector3 pa;
IndexedVector3 pb;
IndexedVector3 Sa;
IndexedVector3 Sb;
if (code <= 3)
{
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else
{
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
IndexedVector3 normal2;
IndexedVector3 nr = new IndexedVector3();
IndexedVector3 anr;
if (code <= 3)
{
normal2 = normal;
}
else
{
normal2 = -normal;
}
DMULTIPLY1_331(ref nr, Rb, ref normal2);
nr.Abs(out anr);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr, a1, a2;
if (anr[1] > anr[0])
{
if (anr[1] > anr[2])
{
a1 = 0;
lanr = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else
{
if (anr[0] > anr[2])
{
lanr = 0;
a1 = 1;
a2 = 2;
}
else
{
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
IndexedVector3 center = new IndexedVector3(); ;
if (nr[lanr] < 0)
{
for (int i = 0; i < 3; i++)
{
center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr];
}
}
else
{
for (int i = 0; i < 3; i++)
{
center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr];
}
}
// find the normal and non-normal axis numbers of the reference box
int codeN, code1, code2;
if (code <= 3)
{
codeN = code - 1;
}
else
{
codeN = code - 4;
}
if (codeN == 0)
{
code1 = 1;
code2 = 2;
}
else if (codeN == 1)
{
code1 = 0;
code2 = 2;
}
else
{
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
float[] quad = s_quad; // 2D coordinate of incident face (x,y pairs)
float c1, c2, m11, m12, m21, m22;
c1 = DDOT14(ref center, 0, Ra, code1);
c2 = DDOT14(ref center, 0, Ra, code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = DDOT44(Ra, code1, Rb, a1);
m12 = DDOT44(Ra, code1, Rb, a2);
m21 = DDOT44(Ra, code2, Rb, a1);
m22 = DDOT44(Ra, code2, Rb, a2);
{
float k1 = m11 * Sb[a1];
float k2 = m21 * Sb[a1];
float k3 = m12 * Sb[a2];
float k4 = m22 * Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
float[] rect = new float[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
float[] ret = s_ret;
int n = IntersectRectQuad2(rect, quad, ret);
if (n < 1)
{
return 0; // this should never happen
}
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
float[] point = s_point; // penetrating contact points
float[] dep = s_dep; // depths for those points
float det1 = 1f / (m11 * m22 - m12 * m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (int j = 0; j < n; j++)
{
float k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2);
float k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2);
for (int i = 0; i < 3; i++)
{
point[cnum * 3 + i] = center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2];
}
dep[cnum] = Sa[codeN] - DDOT(ref normal2, 0, point, cnum * 3);
if (dep[cnum] >= 0)
{
ret[cnum * 2] = ret[j * 2];
ret[cnum * 2 + 1] = ret[j * 2 + 1];
cnum++;
}
}
if (cnum < 1)
{
return 0; // this should never happen
}
// we can't generate more contacts than we actually have
if (maxc > cnum)
{
maxc = cnum;
}
if (maxc < 1)
{
maxc = 1;
}
if (cnum <= maxc)
{
if (code < 4)
{
// we have less contacts than we need, so we use them all
for (int j = 0; j < cnum; j++)
{
IndexedVector3 pointInWorldFA = new IndexedVector3(); ;
for (int i = 0; i < 3; i++)
{
pointInWorldFA[i] = point[j * 3 + i] + pa[i];
}
Vector3 pointInWorld = pointInWorldFA.ToVector3();
if (BulletGlobals.g_streamWriter != null && debugBoxBox)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
}
output.AddContactPoint((-normal).ToVector3(), pointInWorld, -dep[j]);
}
}
else
{
// we have less contacts than we need, so we use them all
for (int j = 0; j < cnum; j++)
{
IndexedVector3 pointInWorld = new IndexedVector3(); ;
for (int i = 0; i < 3; i++)
{
pointInWorld[i] = point[j * 3 + i] + pa[i];
}
if (BulletGlobals.g_streamWriter != null && debugBoxBox)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld.ToVector3());
}
output.AddContactPoint((-normal).ToVector3(), pointInWorld.ToVector3(), -dep[j]);
}
}
}
else
{
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
float maxdepth = dep[0];
for (int i = 1; i < cnum; i++)
{
if (dep[i] > maxdepth)
{
maxdepth = dep[i];
i1 = i;
}
}
int[] iret = new int[8];
CullPoints2(cnum, ret, maxc, i1, iret);
for (int j = 0; j < maxc; j++)
{
// dContactGeom *con = CONTACT(contact,skip*j);
// for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
// con->depth = dep[iret[j]];
IndexedVector3 posInWorldFA = new IndexedVector3(); ;
for (int i = 0; i < 3; i++)
{
posInWorldFA[i] = point[iret[j] * 3 + i] + pa[i];
}
Vector3 pointInWorld = posInWorldFA.ToVector3();
if (BulletGlobals.g_streamWriter != null && debugBoxBox)
{
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "boxbox get closest", pointInWorld);
}
output.AddContactPoint((-normal).ToVector3(), pointInWorld, -dep[iret[j]]);
}
cnum = maxc;
}
return_code = code;
return cnum;
}