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); }