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