cpPolyShapeCacheData(cpPolyShape poly, cpVect p, cpVect rot) { cpPolyShapeTransformAxes(poly, p, rot); cpBB bb = poly.shape.bb = cpPolyShapeTransformVerts(poly, p, rot); return bb; }
cpPolyShapeNearestPointQuery(cpPolyShape poly, cpVect p, cpNearestPointQueryInfo* info) { int count = poly.numVerts; cpSplittingPlane planes = poly.tPlanes; cpVect[] verts = poly.tVerts; cpVect v0 = verts[count - 1]; double minDist = double.PositiveInfinity; cpVect closestPoint = cpvzero; bool outside = false; for (int i = 0; i < count; i++) { if (cpSplittingPlaneCompare(planes[i], p) > 0.0f) outside = true; cpVect v1 = verts[i]; cpVect closest = cpClosetPointOnSegment(p, v0, v1); double dist = cpVect.Distance(p, closest); if (dist < minDist) { minDist = dist; closestPoint = closest; } v0 = v1; } info.shape = (cpShape)poly; info.p = closestPoint; // TODO div/0 info.d = (outside ? minDist : -minDist); }
cpPolyShapeTransformAxes(cpPolyShape poly, cpVect p, cpVect rot) { cpSplittingPlane src = poly.planes; cpSplittingPlane dst = poly.tPlanes; for (int i = 0; i < poly.numVerts; i++) { cpVect n = cpvrotate(src[i].n, rot); dst[i].n = n; dst[i].d = cpVect.Dot(p, n) + src[i].d; } }
cpMomentForPoly(double m, int numVerts, cpVect[] verts, cpVect offset) { double sum1 = 0.0f; double sum2 = 0.0f; for(int i=0; i<numVerts; i++){ cpVect v1 = cpVect.Add(verts[i], offset); cpVect v2 = cpVect.Add(verts[(i+1)%numVerts], offset); double a = cpVect.CrossProduct(v2, v1); double b = cpVect.Dot(v1, v1) + cpVect.Dot(v1, v2) + cpVect.Dot(v2, v2); sum1 += a*b; sum2 += a; } return (m*sum1)/(6.0f*sum2); }
circle2circleQuery(cpVect p1, cpVect p2, double r1, double r2, cpContact con) { double mindist = r1 + r2; cpVect delta = cpVect.Sub(p2, p1); double distsq = cpVect.LengthSQ(delta); if(distsq >= mindist*mindist) return 0; double dist = System.Math.Sqrt(distsq); // Allocate and initialize the contact. cpContactInit( con, cpVect.Add(p1, cpVect.Multiply(delta, 0.5f + (r1 - 0.5f*mindist)/(dist ? dist : double.PositiveInfinity))), (dist ? cpVect.Multiply(delta, 1.0f/dist) : cpv(1.0f, 0.0f)), dist - mindist, 0 ); return 1; }
cpPolyShapeTransformVerts(cpPolyShape poly, cpVect p, cpVect rot) { cpVect[] src = poly.verts; cpVect[] dst = poly.tVerts; double l = double.PositiveInfinity, r = double.NegativeInfinity; double b = double.PositiveInfinity, t = double.NegativeInfinity; for (int i = 0; i < poly.numVerts; i++) { cpVect v = cpVect.Add(p, cpvrotate(src[i], rot)); dst[i] = v; l = System.Math.Min(l, v.x); r = System.Math.Max(r, v.x); b = System.Math.Min(b, v.y); t = System.Math.Max(t, v.y); } return cpBBNew(l, b, r, t); }
static void SWAP(ref cpVect a, ref cpVect b) { cpVect h = a; a = b; b = h; }
QHullReduce(double tol, cpVect[] verts, int offset, int count, cpVect a, cpVect pivot, cpVect b, cpVect[] result, int roffset) { if(count < 0){ return 0; } else if(count == 0) { result[roffset] = pivot; return 1; } else { int left_count = QHullPartition(verts, offset, count, a, pivot, tol); int index = QHullReduce(tol, verts, offset + 1, left_count - 1, a, verts[offset], pivot, result, roffset); result[roffset + index] = pivot; index++; int right_count = QHullPartition(verts, offset, offset + left_count, count - left_count, pivot, b, tol); return index + QHullReduce(tol, verts, offset + left_count + 1, right_count - 1, pivot, verts[offset + left_count], b, result, index); } }
QHullPartition(cpVect[] verts, int count, cpVect a, cpVect b, double tol) { if(count == 0) return 0; double max = 0; int pivot = 0; cpVect delta = cpVect.Sub(b, a); double valueTol = tol*delta.Length; int head = 0; for(int tail = count-1; head <= tail;){ double value = cpVect.CrossProduct(delta, cpVect.Sub(verts[head], a)); if(value > valueTol){ if(value > max){ max = value; pivot = head; } head++; } else { SWAP(ref verts[head], ref verts[tail]); tail--; } } // move the new pivot to the front if it's not already there. if(pivot != 0) SWAP(ref verts[0], ref verts[pivot]); return head; }
cpLoopIndexes(cpVect[] verts, int count, ref int start, ref int end) { start = 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 = v; start = i; } else if(v.x > max.x || (v.x == max.x && v.y > max.y)){ max = v; end = i; } } }
cpBodyGetVelAtWorldPoint(cpBody body, cpVect point) { return cpBodyGetVelAtPoint(body, cpVect.Sub(point, body.p)); }
cpPolyShapeSegmentQuery(cpPolyShape poly, cpVect a, cpVect b, cpSegmentQueryInfo* info) { cpSplittingPlane axes = poly.tPlanes; cpVect[] verts = poly.tVerts; int numVerts = poly.numVerts; for (int i = 0; i < numVerts; i++) { cpVect n = axes[i].n; double an = cpVect.Dot(a, n); if (axes[i].d > an) continue; double bn = cpVect.Dot(b, n); double t = (axes[i].d - an) / (bn - an); if (t < 0.0f || 1.0f < t) continue; cpVect point = cpvlerp(a, b, t); double dt = -cpVect.CrossProduct(n, point); double dtMin = -cpVect.CrossProduct(n, verts[i]); double dtMax = -cpVect.CrossProduct(n, verts[(i + 1) % numVerts]); if (dtMin <= dt && dt <= dtMax) { info.shape = (cpShape)poly; info.t = t; info.n = n; } } }
cpMomentForSegment(double m, cpVect a, cpVect b) { cpVect offset = cpVect.Multiply(cpVect.Add(a, b), 0.5); return m*(cpVect.DistanceSQ(b, a)/12.0f + offset.LengthSQ); }
cpConvexHull(int count, cpVect[] verts, int offset, cpVect[] result, ref int first, double tol) { if(result != null){ // Copy the line vertexes into the empty part of the result polyline to use as a scratch buffer. Array.Copy(verts, offset, result, 0, count); verts.CopyTo(result, count); } else { // If a result array was not specified, reduce the input instead. verts.CopyTo(result, 0); } // Degenerate case, all poins are the same. int start, end; cpLoopIndexes(verts, count, ref start, ref end); if(start == end){ first = 0; return 1; } SWAP(ref result[0], ref result[start]); SWAP(ref result[1], ref result[end == 0 ? start : end]); cpVect a = result[0]; cpVect b = result[1]; first = start; int resultCount = QHullReduce(tol, result, 2, count - 2, a, b, a, result, 1) + 1; return resultCount; }
cpPolyShapeSetVerts(cpShape shape, int numVerts, cpVect[] verts, cpVect offset) { // cpAssertHard(shape.klass == &polyClass, "Shape is not a poly shape."); cpPolyShapeDestroy((cpPolyShape)shape); setUpVerts((cpPolyShape)shape, numVerts, verts, offset); }
cpBoxShapeInit2(cpPolyShape poly, cpBody body, cpBB box) { cpVect[] verts = new cpVect[] { cpv(box.l, box.b), cpv(box.l, box.t), cpv(box.r, box.t), cpv(box.r, box.b) }; return cpPolyShapeInit(poly, body, 4, verts, cpvzero); }
cpPolyShapeNew(cpBody body, int numVerts, cpVect[] verts, cpVect offset) { return (cpShape)cpPolyShapeInit(new cpPolyShape(), body, numVerts, verts, offset); }
cpPolyShapeInit(cpPolyShape poly, cpBody body, int numVerts, cpVect[] verts, cpVect offset) { setUpVerts(poly, numVerts, verts, offset); cpShapeInit((cpShape)poly, &polyClass, body); return poly; }
setUpVerts(cpPolyShape poly, int numVerts, cpVect[] verts, cpVect offset) { // Fail if the user attempts to pass a concave poly, or a bad winding. // cpAssertHard(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding. Consider using cpConvexHull() or CP_CONVEX_HULL()."); poly.numVerts = numVerts; poly.verts = new cpVect[2 * numVerts]; poly.planes = new cpSplittingPlane[2 * numVerts]; poly.tVerts = poly.verts + numVerts; poly.tPlanes = poly.planes + numVerts; for (int i = 0; i < numVerts; i++) { cpVect a = cpVect.Add(offset, verts[i]); cpVect b = cpVect.Add(offset, verts[(i + 1) % numVerts]); cpVect n = cpvnormalize(cpvperp(cpVect.Sub(b, a))); poly.verts[i] = a; poly.planes[i].n = n; poly.planes[i].d = cpVect.Dot(n, a); } }
cpPolyValidate(cpVect[] verts, int numVerts) { for (int i = 0; i < numVerts; i++) { cpVect a = verts[i]; cpVect b = verts[(i + 1) % numVerts]; cpVect c = verts[(i + 2) % numVerts]; if (cpVect.CrossProduct(cpVect.Sub(b, a), cpVect.Sub(c, a)) > 0.0f) { return false; } } return true; }
cpMomentForCircle(double m, double r1, double r2, cpVect offset) { return m*(0.5f*(r1*r1 + r2*r2) + offset.LengthSQ); }
findVerts(cpContact arr, cpPolyShape poly1, cpPolyShape poly2, cpVect n, double dist) { int num = 0; for(int i=0; i<poly1.numVerts; i++){ cpVect v = poly1.tVerts[i]; if(cpPolyShapeContainsVert(poly2, v)) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1.shape.hashid, i)); } for(int i=0; i<poly2.numVerts; i++){ cpVect v = poly2.tVerts[i]; if(cpPolyShapeContainsVert(poly1, v)) cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2.shape.hashid, i)); } return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist)); }
cpAreaForSegment(cpVect a, cpVect b, double r) { return r*((double)System.Math.PI*r + 2.0*cpVect.Distance(a, b)); }
cpBodyGetVelAtLocalPoint(cpBody body, cpVect point) { return cpBodyGetVelAtPoint(body, cpvrotate(point, body.rot)); }
cpAreaForPoly(int numVerts, cpVect[] verts) { double area = 0.0f; for(int i=0; i<numVerts; i++){ area += cpVect.CrossProduct(verts[i], verts[(i+1)%numVerts]); } return -area/2.0f; }
cpCentroidForPoly(int numVerts, cpVect[] verts) { double sum = 0.0f; cpVect vsum = cpvzero; for(int i=0; i<numVerts; i++){ cpVect v1 = verts[i]; cpVect v2 = verts[(i+1)%numVerts]; double cross = cpVect.CrossProduct(v1, v2); sum += cross; vsum = cpVect.Add(vsum, cpVect.Multiply(cpVect.Add(v1, v2), cross)); } return cpVect.Multiply(vsum, 1.0f/(3.0f*sum)); }
segValueOnAxis(cpSegmentShape seg, cpVect n, double d) { double a = cpVect.Dot(n, seg.ta) - seg.r; double b = cpVect.Dot(n, seg.tb) - seg.r; return System.Math.Min(a, b) - d; }
cpRecenterPoly(int numVerts, cpVect[] verts){ cpVect centroid = cpCentroidForPoly(numVerts, verts); for(int i=0; i<numVerts; i++){ verts[i] = cpVect.Sub(verts[i], centroid); } }
cpSpacePointQuery(cpSpace space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, object data) {
cpBodyGetVelAtPoint(cpBody body, cpVect r) { return cpVect.Add(body.v, cpVect.Multiply(cpvperp(r), body.w)); }