/// Get the inverse of a transform matrix.
public static cpTransform Inverse(cpTransform t)
{
float inv_det = 1.0f / (t.a * t.d - t.c * t.b);
return NewTranspose(
t.d * inv_det, -t.c * inv_det, (t.c * t.ty - t.tx * t.d) * inv_det,
-t.b * inv_det, t.a * inv_det, (t.tx * t.b - t.a * t.ty) * inv_det
);
}
/// Transform a cpBB.
public static cpBB BB(cpTransform t, cpBB bb)
{
cpVect center = cpBB.Center(bb);
float hw = (bb.r - bb.l) * 0.5f;
float hh = (bb.t - bb.b) * 0.5f;
float a = t.a * hw, b = t.c * hh, d = t.b * hw, e = t.d * hh;
float hw_max = cp.cpfmax(cp.cpfabs(a + b), cp.cpfabs(a - b));
float hh_max = cp.cpfmax(cp.cpfabs(d + e), cp.cpfabs(d - e));
return cpBB.NewForExtents(Point(t, center), hw_max, hh_max);
}
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);
}
public override cpBB CacheData(cpTransform transform)
{
int count = this.Count;
int dst = 0;
int src = dst + count;
float l = cp.Infinity, r = -cp.Infinity;
float b = cp.Infinity, t = -cp.Infinity;
for (int i = 0; i < count; i++)
{
cpVect v = cpTransform.Point(transform, this.planes[src + i].v0);
cpVect n = cpTransform.Vect(transform, this.planes[src + i].n);
this.planes[i + dst] = new cpSplittingPlane(n, v);
l = cp.cpfmin(l, v.x);
r = cp.cpfmax(r, v.x);
b = cp.cpfmin(b, v.y);
t = cp.cpfmax(t, v.y);
}
float radius = this.r;
return (this.bb = new cpBB(l - radius, b - radius, r + radius, t + radius));
}
public static cpVect[] GetVertices(cpTransform transform, int count, cpVect[] verts)
{
cpVect[] hullVerts = new cpVect[count];
for (int i = 0; i < count; i++)
{
hullVerts[i] = cpTransform.Point(transform, verts[i]);
}
return hullVerts;
}
public override cpBB CacheData(cpTransform transform)
{
cpVect c = this.tc = cpTransform.Point(transform, this.c);
return(new cpBB(c, this.r));
}
public virtual cpBB CacheData(cpTransform transform)
{
throw new NotImplementedException();
}
/// Fast inverse of a rigid transformation matrix.
public static cpTransform RigidInverse(cpTransform t)
{
return NewTranspose(
t.d, -t.c, (t.c * t.ty - t.tx * t.d),
-t.b, t.a, (t.tx * t.b - t.a * t.ty)
);
}
public virtual cpBB CacheData(cpTransform transform)
{
throw new NotImplementedException();
}
public override cpBB CacheData(cpTransform transform)
{
cpVect c = this.tc = cpTransform.Point(transform, this.c);
return new cpBB(c, this.r);
}
/// Construct a new transform matrix.
/// (a, b) is the x basis vector.
/// (c, d) is the y basis vector.
/// (tx, ty) is the translation.
public static cpTransform New(float a, float b, float c, float d, float tx, float ty)
{
cpTransform t = new cpTransform(a, b, c, d, tx, ty);
return t;
}
/// Multiply two transformation matrices.
public static cpTransform Mult(cpTransform t1, cpTransform t2)
{
return NewTranspose(
t1.a * t2.a + t1.c * t2.b, t1.a * t2.c + t1.c * t2.d, t1.a * t2.tx + t1.c * t2.ty + t1.tx,
t1.b * t2.a + t1.d * t2.b, t1.b * t2.c + t1.d * t2.d, t1.b * t2.tx + t1.d * t2.ty + t1.ty
);
}
public static cpTransform WrapInverse(cpTransform outer, cpTransform inner)
{
return Mult(outer, Mult(inner, Inverse(outer)));
}
/// Transform a vector (i.e. a normal)
public static cpVect Vect(cpTransform t, cpVect v)
{
return new cpVect(t.a * v.x + t.c * v.y, t.b * v.x + t.d * v.y);
}
public void SetVerts(int count, cpVect[] verts, cpTransform transform)
{
for (int i = 0; i < verts.Length; i++)
verts[i] = cpTransform.Point(transform, verts[i]);
SetVertsRaw(count, verts);
}
/// <summary>
/// CREATES A BODY WITH MASS AND INERTIA
/// </summary>
/// <param name="mass"></param>
/// <param name="moment"></param>
public cpBody(float mass, float moment)
{
transform = new cpTransform();
this.cog = cpVect.Zero;
this.space = null;
this.shapeList = null;
this.arbiterList = null; // These are both wacky linked lists.
this.constraintList = null;
velocity_func = UpdateVelocity;
position_func = UpdatePosition;
// This stuff is used to track information on the collision graph.
this.nodeRoot = null;
this.nodeNext = null;
this.nodeIdleTime = 0;
/// Position of the rigid body's center of gravity.
this.p = cpVect.Zero;
/// Velocity of the rigid body's center of gravity.
this.v = cpVect.Zero;
/// Force acting on the rigid body's center of gravity.
this.f = cpVect.Zero;
/// Angular velocity of the body around it's center of gravity in radians/second.
this.w = 0;
/// Torque applied to the body around it's center of gravity.
this.t = 0;
// This stuff is all private.
this.v_bias = cpVect.Zero; //x = this.v_biasy = 0;
this.w_bias = 0;
this.userData = null;
this.SetMass(mass);
this.SetMoment(moment);
this.SetAngle(0.0f);
}
// 'p' is the position of the CoG
public void SetTransform(cpVect p, float a)
{
cpVect rot = cpVect.cpvforangle(a);
cpVect c = this.cog;
this.transform = cpTransform.NewTranspose(
rot.x, -rot.y, p.x - (c.x * rot.x - c.y * rot.y),
rot.y, rot.x, p.y - (c.x * rot.y + c.y * rot.x)
);
}
public override cpBB CacheData(cpTransform transform)
{
this.ta = cpTransform.Point(transform, this.a);
this.tb = cpTransform.Point(transform, this.b);
this.tn = cpTransform.Vect(transform, this.n);
float l, r, b, t;
if (this.ta.x < this.tb.x)
{
l = this.ta.x;
r = this.tb.x;
}
else
{
l = this.tb.x;
r = this.ta.x;
}
if (this.ta.y < this.tb.y)
{
b = this.ta.y;
t = this.tb.y;
}
else
{
b = this.tb.y;
t = this.ta.y;
}
float rad = this.r;
return new cpBB(l - rad, b - rad, r + rad, t + rad);
}
public cpBB Update(cpTransform transform)
{
return(this.bb = CacheData(transform));
}
public cpBB Update(cpTransform transform)
{
return (this.bb = CacheData(transform));
}
/// Transform an absolute point. (i.e. a vertex)
public static cpVect Point(cpTransform t, cpVect p)
{
return new cpVect(t.a * p.x + t.c * p.y + t.tx, t.b * p.x + t.d * p.y + t.ty);
}