private bool Gjk(Collider colliderA, Collider colliderB, out ContactData contactData) { contactData = default; Vec3f a, b, c, d; Vec3f Asa, Asb, Asc, Asd, Bsa, Bsb, Bsc, Bsd; a = b = c = d = Vec3f.Zero; Asa = Asb = Asc = Asd = Vec3f.Zero; Bsa = Bsb = Bsc = Bsd = Vec3f.Zero; Vec3f searchDir = colliderA.localCentroid - colliderB.localCentroid; CsoSupport(colliderB, colliderA, searchDir, out c, out Asc, out Bsc); searchDir = -c; CsoSupport(colliderB, colliderA, searchDir, out b, out Asb, out Bsb); if (Vec3f.Dot(b, searchDir) < 0) { return(false); } Vec3f bc = c - b; searchDir = Vec3f.Cross(Vec3f.Cross(bc, -b), bc); //search perpendicular to line segment towards origin if (searchDir.SqrMagnitude < MathUtils.Epsilon) { searchDir = Vec3f.Cross(bc, Vec3f.Right); //normal with x-axis if (Vec3f.Aproximates(searchDir, Vec3f.Zero)) { searchDir = Vec3f.Cross(bc, -Vec3f.Forward); //normal with z-axis } } int simplexDimension = 2; for (int i = 0; i < 64; i++) { CsoSupport(colliderB, colliderA, searchDir, out a, out Asa, out Bsa); if (Vec3f.Dot(a, searchDir) < 0) { return(false); } simplexDimension++; if (simplexDimension == 3) { UpdateSimplex3(ref a, ref b, ref c, ref d, ref simplexDimension, ref searchDir, ref Asa, ref Asb, ref Asc, ref Asd , ref Bsa, ref Bsb, ref Bsc, ref Bsd); } else if (UpdateSimplex4(ref a, ref b, ref c, ref d, ref simplexDimension, ref searchDir, ref Asa, ref Asb, ref Asc, ref Asd , ref Bsa, ref Bsb, ref Bsc, ref Bsd)) { contactData = Epa(colliderA, colliderB, a, b, c, d, Asa, Asb, Asc, Asd, Bsa, Bsb, Bsc, Bsd); return(true); } } return(false); }
public static Quatf operator *(Quatf lhs, Quatf rhs) { Vec3f lxyz = lhs.XYZ; Vec3f rxyz = rhs.XYZ; return(new Quatf( (rhs.w * lxyz) + (lhs.w * rxyz) + Vec3f.Cross(lxyz, rxyz), (lhs.w * rhs.w) - Vec3f.Dot(lxyz, rxyz))); }
public Vec3f Normal(int edgeIdx) { Vec3f d1 = GetEdgeDelta(edgeIdx); Vec3f d2 = GetEdgeDelta(Next(edgeIdx)); Vec3f norm = Vec3f.Cross(d1, d2); return(norm.Normalized()); }
public static Vec3f operator *(Quatf q, Vec3f v) { var QuatVector = new Vec3f(q.x, q.y, q.z); var uv = Vec3f.Cross(QuatVector, v); var uuv = Vec3f.Cross(QuatVector, uv); uv *= (2.0f * q.w); uuv *= 2.0f; return(v + uv + uuv); }
public static bool ContainsPoint(Vec3f a, Vec3f b, Vec3f c, Vec3f point) { a -= point; b -= point; c -= point; Vec3f u = Vec3f.Cross(b, c); Vec3f v = Vec3f.Cross(c, a); Vec3f w = Vec3f.Cross(a, b); return(Vec3f.Dot(u, v) >= 0f && Vec3f.Dot(u, w) > 0f); }
public Vec3f Normal(int vIdx1, int vIdx2, int vIdx3) { var v1 = Points[vIdx1]; var v2 = Points[vIdx2]; var v3 = Points[vIdx3]; Vec3f d1 = v2 - v1; Vec3f d2 = v3 - v1; Vec3f norm = Vec3f.Cross(d1, d2); return(norm.Normalized()); }
public ContactData(Vec3f worldPointA, Vec3f localPointA, Vec3f worldPointB, Vec3f localPointB, Vec3f normal, float depth) { this.localPointA = localPointA; this.localPointB = localPointB; this.worldPointA = worldPointA; this.worldPointB = worldPointB; this.depth = depth; this.normal = normal; if (normal.x >= 0.57735f) { tang1 = new Vec3f(normal.y, -normal.x, 0); } else { tang1 = new Vec3f(0.0f, normal.z, -normal.y); } tang1.Normalize(); tang2 = Vec3f.Cross(normal, tang1); }
public bool SameDirEdges(int edge1, int edge2) { Vec3f d1 = GetEdgeDelta(edge1); Vec3f d2 = GetEdgeDelta(edge2); if (Vec3f.Dot(d1, d2) < 0) { return(false); } float c = Vec3f.Cross(d1, d2).MagnitudeSqrd(); const float kEdgeThresh = 0.01f; if (c >= kEdgeThresh * kEdgeThresh) { return(false); } else { return(true); } }
public Plane(Vec3f a, Vec3f b, Vec3f c) { normal = Vec3f.Cross(a - b, a - c).Normalized(); distance = Vec3f.Dot(normal, a); }
public Vec3f GetNormal() => Vec3f.Cross(b - a, c - a).Normalized();