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