public cpShapeMassInfo(float m, float i, cpVect cog, float area)
{
this.m = m;
this.i = i;
this.cog = cog;
this.area = area;
}
public cpPivotJoint(cpBody a, cpBody b, cpVect anchorA, cpVect anchorB)
: base(a, b)
{
this.anchorA = anchorA;
this.anchorB = anchorB;
this.jAcc = cpVect.Zero;
}
public PointQueryContext(cpVect point1, float maxDistance1, cpShapeFilter filter1, Action<cpShape, cpVect, float, cpVect, object> func1)
{
// TODO: Complete member initialization
this.point = point1;
this.maxDistance = maxDistance1;
this.filter = filter1;
this.func = func1;
}
public static cpTransform BoneScale(cpVect v0, cpVect v1)
{
cpVect d = cpVect.cpvsub(v1, v0);
return NewTranspose(
d.x, -d.y, v0.x,
d.y, d.x, v0.y
);
}
public CCRayCastCallbackInfo(CCPhysicsWorld world, Func<CCPhysicsWorld, CCPhysicsRayCastInfo, object, bool> func, cpVect p1, cpVect p2, object data)
{
this.world = world;
this.func = func;
this.p1 = p1;
this.p2 = p2;
this.data = data;
}
public static cpShapeMassInfo cpCircleShapeMassInfo(float mass, float radius, cpVect center)
{
var info = new cpShapeMassInfo(
mass, cp.MomentForCircle(1.0f, 0.0f, radius, cpVect.Zero),
center,
cp.AreaForCircle(0.0f, radius)
);
return info;
}
public cpPointQueryExtendedInfo(cpShape tShape)
{
/// The nearest shape, NULL if no shape was within range.
this.shape = tShape;
/// The closest point on the shape's surface. (in world space coordinates)
this.d = cp.Infinity;
/// The distance to the point. The distance is negative if the point is inside the shape.
this.n = cpVect.Zero;
}
public void Set(cpPointQueryInfo newPointInfo)
{
/// The nearest shape, NULL if no shape was within range.
shape = newPointInfo.shape;
point = newPointInfo.point;
distance = newPointInfo.distance;
gradient = newPointInfo.gradient;
// g = newPointInfo.g;
}
public static cpTransform AxialScale(cpVect axis, cpVect pivot, float scale)
{
float A = axis.x * axis.y * (scale - 1.0f);
float B = cpVect.cpvdot(axis, pivot) * (1.0f - scale);
return NewTranspose(
scale * axis.x * axis.x + axis.y * axis.y, A, axis.x * B,
A, axis.x * axis.x + scale * axis.y * axis.y, axis.y * B
);
}
public cpSlideJoint(cpBody a, cpBody b, cpVect anchorA, cpVect anchorB, float min, float max)
: base(a, b)
{
this.anchorA = anchorA;
this.anchorB = anchorB;
this.min = min;
this.max = max;
this.jnAcc = 0.0f;
}
public cpSegmentQueryInfo(cpShape shape, cpVect point, cpVect normal, float alpha)
{
/// The shape that was hit, NULL if no collision occured.
this.shape = shape;
/// The normalized distance along the query segment in the range [0, 1].
this.alpha = alpha;
/// The normal of the surface hit.
this.normal = normal;
this.point = point;
}
public cpPointQueryInfo(cpShape shape, cpVect point, float distance, cpVect gradient)
{
/// The nearest shape, NULL if no shape was within range.
this.shape = shape;
/// The closest point on the shape's surface. (in world space coordinates)
this.point = point;
/// The distance to the point. The distance is negative if the point is inside the shape.
this.distance = distance;
this.gradient = gradient;
}
public static cpShapeMassInfo cpPolyShapeMassInfo(float mass, int count, cpVect[] verts, float radius)
{
cpVect centroid = cp.CentroidForPoly(count, verts);
var info = new cpShapeMassInfo(
mass,
cp.MomentForPoly(1.0f, count, verts, cpVect.cpvneg(centroid), radius),
centroid,
cp.AreaForCircle(0.0f, radius)
);
return info;
}
public b2dJsonImage()
{
Body = null;
Center = cpVect.Zero;
Angle = 0; Scale = 1;
Flip = false;
Filter = _b2dJsonImagefilterType.FT_LINEAR;
ColorTint = new int[4];
m_corners = new cpVect[4];
}
static CCPoint[] cpVertArray2ccpArrayN(cpVect[] cpVertArray, int count)
{
if (count == 0)
return null;
CCPoint[] pPoints = new CCPoint[count];
for (int i = 0; i < count; ++i)
{
pPoints[i].X = cpVertArray[i].x;
pPoints[i].Y = cpVertArray[i].y;
}
return pPoints;
}
public cpPolyShape(cpBody body, int count, cpVect[] verts,
cpTransform transform, float radius)
: base(body, new cpShapeMassInfo())
{
cpVect[] hullVerts = new cpVect[count];
// Transform the verts before building the hull in case of a negative scale.
for (int i = 0; i < count; i++)
hullVerts[i] = cpTransform.Point(transform, verts[i]);
int hullCount = cp.ConvexHull(count, hullVerts, ref hullVerts, null, 0.0f);
InitRaw(hullCount, hullVerts, radius);
}
internal static CCPoint[] cpVectsTpCCPoints (cpVect[] points)
{
var len = points.Length;
var pointscp = new CCPoint[len];
var wrkPoint = cpVect.Zero;
for (int i = 0; i< len; i++)
{
wrkPoint = points[i];
pointscp[i] = new CCPoint(wrkPoint.x, wrkPoint.y);
}
return pointscp;
}
public cpPolyShape ToShape(cpBody body, float radius, cpVect size, int shapeIdx = 0)
{
var source = Shapes[shapeIdx];
var vertices = new cpVect[source.Length];
for (int i = 0, count = source.Length; i < count; i++)
{
var sourceVect = source[i];
var vect = cpVect.cpv(sourceVect.x * size.x, sourceVect.y * size.y);
vertices[i] = vect;
}
return new cpPolyShape(body, vertices.Length, vertices, radius);
}
internal static cpVect[] CCPointsTocpVects (CCPoint[] points)
{
var len = points.Length;
cpVect[] pointscp = new cpVect[len];
var wrkPoint = CCPoint.Zero;
for (int i = 0; i< len; i++)
{
wrkPoint = points[i];
pointscp[i] = new cpVect(wrkPoint.X, wrkPoint.Y);
}
return pointscp;
}
public cpGrooveJoint(cpBody a, cpBody b, cpVect groove_a, cpVect groove_b, cpVect anchorB)
: base(a, b)
{
this.grv_a = groove_a;
this.grv_b = groove_b;
this.grv_n = cpVect.cpvperp(cpVect.cpvnormalize(cpVect.cpvsub(groove_b, groove_a)));
this.anchorB = anchorB;
this.grv_tn = null;
this.clamp = 0.0f;
this.r1 = this.r2 = null;
this.jAcc = cpVect.Zero;
this.bias = null;
}
public CCPhysicsRayCastInfo(CCPhysicsShape shape,
cpVect start,
cpVect end,
cpVect contact,
cpVect normal,
float fraction,
object data)
{
this.shape = shape;
this.start = start;
this.end = end; //< in lua, it's name is "ended"
this.contact = contact;
this.normal = normal;
this.fraction = fraction;
this.data = data;
}
public override void ApplyImpulse(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
cpVect r1 = this.r1;
cpVect r2 = this.r2;
// compute relative velocity
cpVect vr = cp.relative_velocity(a, b, r1, r2);
// compute normal impulse
cpVect j = cpMat2x2.Transform(this.k, cpVect.cpvsub(this.bias, vr));
cpVect jOld = this.jAcc;
this.jAcc = cpVect.cpvclamp(cpVect.cpvadd(this.jAcc, j), this.maxForce * dt);
j = cpVect.cpvsub(this.jAcc, jOld);
// apply impulse
cp.apply_impulses(a, b, this.r1, this.r2, j);
}
public cpDampedSpring(cpBody a, cpBody b, cpVect anchr1, cpVect anchr2, float restLength, float stiffness, float damping)
: base(a, b)
{
this.anchorA = anchr1;
this.anchorB = anchr2;
this.restLength = restLength;
this.stiffness = stiffness;
this.damping = damping;
this.springForceFunc = defaultSpringForce;
this.target_vrn = this.v_coef = 0;
this.r1 = this.r2 = null;
this.nMass = 0;
this.n = null;
this.jAcc = 0f;
}
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, Func <object, object, object, float> 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);
}
}
/// 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 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);
}
//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 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);
}
public static float cpvtoangle(cpVect v)
{
return(cp.cpfatan2(v.y, v.x));
}
static CCPoint cpVert2Point(cpVect vert)
{
return new CCPoint(vert.x, vert.y);
}
public void Draw(cpVect point, float radius)
{
DrawDot(point, radius, cpColor.Red);
}
public void Draw(cpVect point)
{
Draw(point, 0.5f);
}
public void DrawSegment(cpVect from, cpVect to, float radius, cpColor color)
{
base.DrawSegment(from.ToCCPoint(), to.ToCCPoint(), radius, color.ToCCColor4F());
}
public void Draw(cpVect point, float radius, cpColor color)
{
DrawDot(point, radius, color);
}
public void Draw(cpVect point, cpColor color)
{
Draw(point, 0.5f, color);
}
public override void SetAnchorA(cpVect anchr1)
{
ActivateBodies();
this.anchorA = anchr1;
}
public static string cpvstr(cpVect v)
{
return(string.Format("({0:N3}, {1:N3})", v.x, v.y));
}
/// Returns a normalized copy of v.
public static cpVect vnormalize(cpVect v)
{
return(cpvmult(v, 1.0f / cpvlength(v)));
}
/// Linearly interpolate between v1 and v2.
public static cpVect vlerp(cpVect v1, cpVect v2, float t)
{
return(cpvadd(cpvmult(v1, 1 - t), cpvmult(v2, t)));
}
public static bool Equal(cpVect point1, cpVect point2)
{
return((point1.x == point2.x) && (point1.y == point2.y));
}
public cpVect(cpVect pt)
{
x = pt.x;
y = pt.y;
}
public void DrawSegment(cpVect from, cpVect to, float radius, cpColor color)
{
base.DrawSegment(from.ToCCPoint(), to.ToCCPoint(), radius, color.ToCCColor4F());
}
public void Draw(cpVect point)
{
Draw(point, 0.5f);
}
public override void SetAnchorB(cpVect anchr2)
{
ActivateBodies();
this.anchorB = anchr2;
}
public void Draw(cpVect point, float radius)
{
DrawDot(point, radius, cpColor.Red);
}
public cpPivotJoint(cpBody a, cpBody b, cpVect pivot)
: this(a, b,
(a != null ? a.WorldToLocal(pivot) : pivot),
(b != null ? b.WorldToLocal(pivot) : pivot))
{
}
static CCPoint cpVert2Point(cpVect vert)
{
return(new CCPoint(vert.x, vert.y));
}
public static cpVect cpvnormalize_safe(cpVect v)
{
return(v.x == 0.0f && v.y == 0.0f ? cpVect.Zero : cpvnormalize(v));
}
private void DrawFatSegment(cpVect ta, cpVect tb, float r, cpColor color)
{
cpColor fill = new cpColor(color);
fill.a = cp.cpflerp(color.a, 1.0f, 0.5f);
DrawSegment(ta, tb, Math.Max(0.05f, r), fill);
}
public void DrawSolidCircle(cpVect center, float radius, cpColor color)
{
base.DrawCircle(center.ToCCPoint(), radius, color.ToCCColor4B());
}
public void Draw(cpVect point, cpColor color)
{
Draw(point, 0.5f, color);
}
public void DrawCircle(cpVect center, float radius, float angle, int segments, cpColor color)
{
base.DrawCircle(center.ToCCPoint(), radius, angle, segments, color.ToCCColor4B());
}
public void Draw(cpVect point, float radius, cpColor color)
{
DrawDot(point, radius, color);
}
public void DrawDot(cpVect pos, float radius, cpColor color)
{
//base.DrawDot(pos.ToCCPoint(), radius, color.ToCCColor4F());
base.DrawSolidCircle(pos.ToCCPoint(), radius, color.ToCCColor4B());
}
/// Returns the squared distance between v1 and v2. Faster than vdist() when you only need to compare distances.
public static float vdistsq(cpVect v1, cpVect v2)
{
return(cpvlengthsq(cpvsub(v1, v2)));
}
/// Linearly interpolate between v1 towards v2 by distance d.
public static cpVect vlerpconst(cpVect v1, cpVect v2, float d)
{
return(cpvadd(v1, cpvclamp(cpvsub(v2, v1), d)));
}
