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