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);
}
/// Sets damping independantly along the x and y axes
public void SetAxisAligned(float xDamping, float yDamping)
{
T.Col1.X = -xDamping;
T.Col1.Y = 0;
T.Col2.X = 0;
T.Col2.Y = -yDamping;
if (xDamping > 0 || yDamping > 0)
{
MaxTimestep = 1 / MathB2.Max(xDamping, yDamping);
}
else
{
MaxTimestep = 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);
}
public void CreateChain(List <Vec2> vertices)
{
m_vertices = new List <Vec2>();
for (int i = 1; i < vertices.Count; i++)
{
Vec2 v1 = vertices[i - 1];
Vec2 v2 = vertices[i];
if (MathB2.DistanceSquared(v1, v2) > Settings.LinearSlop * Settings.LinearSlop)
{
throw new Exception("Two vertex are too close");
}
}
m_vertices = vertices;
m_hasPrevVertex = false;
m_hasNextVertex = false;
}
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);
}
