public override void ApplyImpulse(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
cpVect n = this.n;
cpVect r1 = this.r1;
cpVect r2 = this.r2;
// compute relative velocity
float vrn = cp.normal_relative_velocity(a, b, r1, r2, n);
// compute velocity loss from drag
float v_damp = (this.target_vrn - vrn) * this.v_coef;
this.target_vrn = vrn + v_damp;
float j_damp = v_damp * this.nMass;
this.jAcc += j_damp;
cp.apply_impulses(a, b, this.r1, this.r2, cpVect.cpvmult(this.n, j_damp));
}
// k1 and k2 are modified by the function to contain the outputs.
public static cpMat2x2 k_tensor(cpBody a, cpBody b, cpVect r1, cpVect r2)
{
float m_sum = a.m_inv + b.m_inv;
// start with Identity*m_sum
float k11 = m_sum, k12 = 0.0f;
float k21 = 0.0f, k22 = m_sum;
// add the influence from r1
float a_i_inv = a.i_inv;
float r1xsq = r1.x * r1.x * a_i_inv;
float r1ysq = r1.y * r1.y * a_i_inv;
float r1nxy = -r1.x * r1.y * a_i_inv;
k11 += r1ysq; k12 += r1nxy;
k21 += r1nxy; k22 += r1xsq;
// add the influnce from r2
float b_i_inv = b.i_inv;
float r2xsq = r2.x * r2.x * b_i_inv;
float r2ysq = r2.y * r2.y * b_i_inv;
float r2nxy = -r2.x * r2.y * b_i_inv;
k11 += r2ysq; k12 += r2nxy;
k21 += r2nxy; k22 += r2xsq;
// invert
float det = k11 * k22 - k12 * k21;
cp.AssertSoft(det != 0.0f, "Unsolvable constraint.");
float det_inv = 1.0f / det;
return(new cpMat2x2(
k22 * det_inv, -k12 * det_inv,
-k21 * det_inv, k11 * det_inv
));
}
public static Edge SupportEdgeForSegment(cpSegmentShape seg, cpVect n)
{
ulong hashid = seg.hashid;
Edge edge;
if (cpVect.cpvdot(seg.tn, n) > 0.0f)
{
edge = new Edge(
new EdgePoint(seg.ta, cp.CP_HASH_PAIR(seg.hashid, 0)),
new EdgePoint(seg.tb, cp.CP_HASH_PAIR(seg.hashid, 1)),
seg.r, seg.tn);
}
else
{
edge = new Edge(
new EdgePoint(seg.tb, cp.CP_HASH_PAIR(seg.hashid, 1)),
new EdgePoint(seg.ta, cp.CP_HASH_PAIR(seg.hashid, 0)),
seg.r, cpVect.cpvneg(seg.tn)
);
}
return(edge);
}
public float SubtreeSegmentQuery(Node subtree, object obj, cpVect a, cpVect b, float t_exit, cpSpatialIndexSegmentQueryFunc func, object data)
{
if (subtree.isLeaf)
{
return(func(obj, subtree.obj, data));
}
else
{
float t_a = subtree.A.bb.SegmentQuery(a, b);
float t_b = subtree.B.bb.SegmentQuery(a, b);
if (t_a < t_b)
{
if (t_a < t_exit)
{
t_exit = cp.cpfmin(t_exit, SubtreeSegmentQuery(subtree.A, obj, a, b, t_exit, func, data));
}
if (t_b < t_exit)
{
t_exit = cp.cpfmin(t_exit, SubtreeSegmentQuery(subtree.B, obj, a, b, t_exit, func, data));
}
}
else
{
if (t_b < t_exit)
{
t_exit = cp.cpfmin(t_exit, SubtreeSegmentQuery(subtree.B, obj, a, b, t_exit, func, data));
}
if (t_a < t_exit)
{
t_exit = cp.cpfmin(t_exit, SubtreeSegmentQuery(subtree.A, obj, a, b, t_exit, func, data));
}
}
return(t_exit);
}
}
public void SetVerts(int count, cpVect[] verts)
{
Count = count;
if (Count <= CP_POLY_SHAPE_INLINE_ALLOC)
{
this.planes = new cpSplittingPlane[2 * CP_POLY_SHAPE_INLINE_ALLOC];
}
else
{
this.planes = new cpSplittingPlane[2 * Count];
}
// This a pretty bad way to do this in javascript. As a first pass, I want to keep
// the code similar to the C.
for (int i = 0; i < Count; i++)
{
cpVect a = verts[(i - 1 + Count) % Count];
cpVect b = verts[i];
cpVect n = cpVect.cpvnormalize(cpVect.cpvrperp(cpVect.cpvsub(b, a)));
this.planes[i + Count] = new cpSplittingPlane(n, b);
}
}
//MARK: Quick Hull
public static void LoopIndexes(ref cpVect[] verts, int count, out int start, out int end)
{
start = 0;
end = 0;
cpVect min = verts[0];
cpVect max = min;
for (int i = 1; i < count; i++)
{
cpVect v = verts[i];
if (v.x < min.x || (v.x == min.x && v.y < min.y))
{
min = verts[i];
start = i;
}
else if (v.x > max.x || (v.x == max.x && v.y > max.y))
{
max = verts[i];
end = i;
}
}
}
public override void PreStep(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
this.r1 = cpTransform.Vect(a.transform, cpVect.cpvsub(this.anchorA, a.cog));
this.r2 = cpTransform.Vect(b.transform, cpVect.cpvsub(this.anchorB, b.cog));
cpVect delta = cpVect.cpvsub(cpVect.cpvadd(b.p, this.r2), cpVect.cpvadd(a.p, this.r1));
float dist = cpVect.cpvlength(delta);
float pdist = 0.0f;
if (dist > this.max)
{
pdist = dist - this.max;
this.n = cpVect.cpvnormalize(delta);
}
else if (dist < this.min)
{
pdist = this.min - dist;
this.n = cpVect.cpvneg(cpVect.cpvnormalize(delta));
}
else
{
this.n = cpVect.Zero;
this.jnAcc = 0.0f;
}
// calculate mass normal
this.nMass = 1.0f / cp.k_scalar(a, b, this.r1, this.r2, this.n);
// calculate bias velocity
float maxBias = this.maxBias;
this.bias = cp.cpfclamp(-cp.bias_coef(this.errorBias, dt) * pdist / dt, -maxBias, maxBias);
}
public bool SegmentQuery(cpVect a, cpVect b, float radius, ref cpSegmentQueryInfo info)
{
if (info == null)
{
info = new cpSegmentQueryInfo(null, b, cpVect.Zero, 1.0f);
}
cpPointQueryInfo nearest = null;
PointQuery(a, ref nearest);
if (nearest.distance <= radius)
{
info.shape = this;
info.alpha = 0.0f;
info.normal = cpVect.cpvnormalize(cpVect.cpvsub(a, nearest.point));
}
else
{
segmentQuery(a, b, radius, ref info);
}
return(info.shape != null);
}
/// Return a contact set from an arbiter.
public cpContactPointSet GetContactPointSet()
{
cpContactPointSet set = new cpContactPointSet();
set.count = GetCount();
set.points = new PointsDistance[set.count];
bool swapped = this.swapped;
set.normal = (swapped ? cpVect.cpvneg(this.n) : this.n);
for (int i = 0; i < set.count; i++)
{
// Contact points are relative to body CoGs;
cpVect p1 = cpVect.cpvadd(this.body_a.p, this.contacts[i].r1);
cpVect p2 = cpVect.cpvadd(this.body_b.p, this.contacts[i].r2);
set.points[i] = new PointsDistance();
set.points[i].pointA = (swapped ? p2 : p1);
set.points[i].pointB = (swapped ? p1 : p2);
set.points[i].distance = cpVect.cpvdot(cpVect.cpvsub(p2, p1), this.n);
}
return(set);
}
public static int QHullReduce(float tol, cpVect[] verts, int verts_index, int count, cpVect a, cpVect pivot, cpVect b, ref cpVect[] result, int result_index)
{
if (count < 0)
{
return(0);
}
else if (count == 0)
{
result[result_index] = pivot;
return(1);
}
else
{
int left_count = QHullPartition(ref verts, verts_index, count, a, pivot, tol);
int index = QHullReduce(tol, verts, verts_index + 1, left_count - 1, a, verts[verts_index], pivot, ref result, result_index);
result[(index++) + result_index] = pivot;
int right_count = QHullPartition(ref verts, verts_index + left_count, count - left_count, pivot, b, tol);
int new_index = verts_index + left_count;
if (new_index > verts.Length - 1)
{
new_index = verts.Length - 1;
}
return(index + QHullReduce(tol, verts, verts_index + left_count + 1, right_count - 1, pivot, verts[new_index], b, ref result, result_index + index));
}
}
public static float MomentForCircle(float m, float r1, float r2, cpVect offset)
{
return(m * (0.5f * (r1 * r1 + r2 * r2) + cpVect.cpvlengthsq(offset)));
}
public static void apply_bias_impulses(cpBody a, cpBody b, cpVect r1, cpVect r2, cpVect j)
{
apply_bias_impulse(a, cpVect.cpvneg(j), r1);
apply_bias_impulse(b, j, r2);
}
public static float normal_relative_velocity(cpBody a, cpBody b, cpVect r1, cpVect r2, cpVect n)
{
return(cpVect.cpvdot(relative_velocity(a, b, r1, r2), n));
}
public static bool CheckArea(cpVect v1, cpVect v2)
{
return((v1.x * v2.y) > (v1.y * v2.x));
}
//MARK: EPA Functions
public static float ClosestDist(cpVect v0, cpVect v1)
{
return(cpVect.cpvlengthsq(LerpT(v0, v1, ClosestT(v0, v1))));
}
public static cpVect frand_unit_circle()
{
cpVect v = new cpVect(frand() * 2.0f - 1.0f, frand() * 2.0f - 1.0f);
return(cpVect.cpvlengthsq(v) < 1.0f ? v : frand_unit_circle());
}
public static void CircleSegmentQuery(cpShape shape, cpVect center, float r1, cpVect a, cpVect b, float r2, ref cpSegmentQueryInfo info)
{
// offset the line to be relative to the circle
cpVect da = cpVect.cpvsub(a, center);
cpVect db = cpVect.cpvsub(b, center);
float rsum = r1 + r2;
float qa = cpVect.cpvdot(da, da) - 2 * cpVect.cpvdot(da, db) + cpVect.cpvdot(db, db);
float qb = cpVect.cpvdot(da, db) - cpVect.cpvdot(da, da);
float det = qb * qb - qa * (cpVect.cpvdot(da, da) - rsum * rsum);
if (det >= 0.0f)
{
float t = (-qb - cp.cpfsqrt(det)) / (qa);
if (0.0f <= t && t <= 1.0f)
{
{
cpVect n = cpVect.cpvnormalize(cpVect.cpvlerp(da, db, t));
info.shape = shape;
info.point = cpVect.cpvsub(cpVect.cpvlerp(da, db, t), cpVect.cpvmult(n, r2));
info.normal = n;
info.alpha = t;
}
}
}
}
public SupportPoint(cpVect p, ulong id)
{
this.p = p;
this.id = id;
}
public static void apply_bias_impulse(cpBody body, cpVect j, cpVect r)
{
body.v_bias = cpVect.cpvadd(body.v_bias, cpVect.cpvmult(j, body.m_inv));
body.w_bias += body.i_inv * cpVect.cpvcross(r, j);
}
public static SupportPoint CircleSupportPoint(cpCircleShape circle, cpVect n)
{
return(new SupportPoint(circle.tc, 0));
}
public static float k_scalar_body(cpBody body, cpVect r, cpVect n)
{
var rcn = cpVect.cpvcross(r, n);
return(body.m_inv + body.i_inv * rcn * rcn);
}
public static SupportPoint PolySupportPoint(cpPolyShape poly, cpVect n)
{
ulong i = PolySupportPointIndex(poly.Count, poly.planes, n);
return(new SupportPoint(poly.planes[i].v0, i));
}
public static cpVect Transform(cpMat2x2 m, cpVect v)
{
return(new cpVect(v.x * m.a + v.y * m.b, v.x * m.c + v.y * m.d));
}
public cpPivotJoint(cpBody a, cpBody b, cpVect pivot)
: this(a, b,
(a != null ? a.WorldToLocal(pivot) : pivot),
(b != null ? b.WorldToLocal(pivot) : pivot))
{
}
public static float AreaForSegment(cpVect a, cpVect b, float r)
{
return(r * (cp.M_PI * r + 2 * cpVect.cpvdist(a, b)));
}
public override void SetAnchorA(cpVect anchr1)
{
ActivateBodies();
this.anchorA = anchr1;
}
private static int QHullPartition(ref cpVect[] verts, int verts_index, int count, cpVect a, cpVect b, float tol)
{
if (count == 0)
{
return(0);
}
float max = 0;
int pivot = 0;
cpVect delta = cpVect.cpvsub(b, a);
float valueTol = tol * cpVect.cpvlength(delta);
int head = 0;
for (int tail = count - 1; head <= tail;)
{
float value = cpVect.cpvcross(cpVect.cpvsub(verts[head + verts_index], a), delta);
if (value > valueTol)
{
if (value > max)
{
max = value;
pivot = head;
}
head++;
}
else
{
SWAP(ref verts[verts_index + head], ref verts[verts_index + tail]);
tail--;
}
}
// move the new pivot to the front if it's not already there.
if (pivot != 0)
{
SWAP(ref verts[verts_index], ref verts[verts_index + pivot]);
}
return(head);
}
public override void SetAnchorB(cpVect anchr2)
{
ActivateBodies();
this.anchorB = anchr2;
}
public override void SetAnchorA(cpVect anchr)
{
ActivateBodies();
anchorA = anchr;
}
public static cpVect point2canvas(cpVect point, float scale)
{
return(new cpVect(point.x * scale, (480 - point.y) * scale));
}
