public void ComputeDistance(XForm xf, Vec2 p, float distance, Vec2 normal, int childIndex) { Vec2 v1 = MathB2.Mul(xf, _v1); Vec2 v2 = MathB2.Mul(xf, _v2); Vec2 d = p - v1; Vec2 s = v2 - v1; float ds = MathB2.Dot(d, s); if (ds > 0) { float s2 = MathB2.Dot(s, s); if (ds > s2) { d = p - v2; } else { d -= ds / s2 * s; } } float d1 = d.Length(); distance = d1; normal = d1 > 0 ? 1 / d1 * d : new Vec2(0, 0); }
public void ComputeAABB(out AABB aabb, XForm xf, int childIndex) { int i1 = childIndex; int i2 = childIndex + 1; if (i2 == m_count) { i2 = 0; } // TODO : Check if the override of the AABB function needed Vec2 v1 = MathB2.Mul(xf, m_vertices[i1]); Vec2 v2 = MathB2.Mul(xf, m_vertices[i2]); aabb.LowerBound = MathB2.Min(v1, v2); aabb.UpperBound = MathB2.Max(v1, v2); }
void ReadCache(SimplexCache cache, DistanceProxy proxyA, XForm transformA, DistanceProxy proxyB, XForm transformB) { // Copy data from cache. m_count = cache.count; SimplexVertex[] vertices = m_v1; for (int i = 0; i < m_count; ++i) { SimplexVertex v = vertices[i]; v.indexA = cache.indexA[i]; v.indexB = cache.indexB[i]; Vec2 wALocal = proxyA.GetVertex(v.indexA); Vec2 wBLocal = proxyB.GetVertex(v.indexB); v.wA = MathB2.Mul(transformA, wALocal); v.wB = MathB2.Mul(transformB, wBLocal); v.w = v.wB - v.wA; v.a = 0.0f; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (m_count > 1) { float metric1 = cache.metric; float metric2 = GetMetric(); if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.FLT_EPSILON) { // Reset the simplex. m_count = 0; } } // If the cache is empty or invalid ... if (m_count == 0) { SimplexVertex v = vertices[0]; v.indexA = 0; v.indexB = 0; Vec2 wALocal = proxyA.GetVertex(0); Vec2 wBLocal = proxyB.GetVertex(0); v.wA = MathB2.Mul(transformA, wALocal); v.wB = MathB2.Mul(transformB, wBLocal); v.w = v.wB - v.wA; v.a = 1.0f; m_count = 1; } }
public override void Step(TimeStep step) { float timestep = step.Dt; if (timestep <= Settings.FLT_EPSILON) { return; } if (timestep > MaxTimestep && MaxTimestep > 0) { timestep = MaxTimestep; } for (ControllerEdge i = _bodyList; i != null; i = i.nextBody) { Body body = i.body; if (body.IsSleeping()) { continue; } Vec2 damping = body.GetWorldVector(MathB2.Mul(T, body.GetLocalVector(body.GetLinearVelocity()))); body.SetLinearVelocity(body.GetLinearVelocity() + timestep * damping); } }
internal bool RayCast(RayCastOutput output, RayCastInput input, XForm xf, int v) { Vec2 p1 = MathB2.MulT(xf.R, input.P1 - xf.Position); Vec2 p2 = MathB2.MulT(xf.R, input.P2 - xf.Position); Vec2 d = p2 - p1; Vec2 v1 = _v1; Vec2 v2 = _v2; Vec2 e = v2 - v1; Vec2 normal = new Vec2(e.Y, -e.X); normal.Normalize(); // q = p1 + t * d // dot(normal, q - v1) = 0 // dot(normal, p1 - v1) + t * dot(normal, d) = 0 float numerator = MathB2.Dot(normal, v1 - p1); float denominator = MathB2.Dot(normal, d); if (denominator == 0.0f) { return(false); } float t = numerator / denominator; if (t < 0.0f || input.MaxFraction < t) { return(false); } Vec2 q = p1 + t * d; // q = v1 + s * r // s = dot(q - v1, r) / dot(r, r) Vec2 r = v2 - v1; float rr = MathB2.Dot(r, r); if (rr == 0.0f) { return(false); } float s = MathB2.Dot(q - v1, r) / rr; if (s < 0.0f || 1.0f < s) { return(false); } output.Fraction = t; if (numerator > 0.0f) { output.Normal = -MathB2.Mul(xf.R, normal); } else { output.Normal = MathB2.Mul(xf.R, normal); } return(true); }