public PBody AddPolygon(bool fix, Polygon p, float angle, float density)
{
PBody body = Polygon(fix, p, angle, density);
world.AddBody(body);
return(body);
}
public PBody AddShape(bool fix, PShape shape, float angle, float density)
{
PBody body = Shape(fix, shape, angle, density);
world.AddBody(body);
return(body);
}
public PBody AddBox(bool fix, RectBox rect, float angle, float density)
{
PBody body = Box(fix, rect, angle, density);
world.AddBody(body);
return(body);
}
public bool RemoveBody(PBody b, bool identity)
{
object[] items = this.bodies;
if (identity || b == null)
{
for (int i = 0; i < numBodies; i++)
{
if (items[i] == (object)b)
{
RemoveBody(i);
return(true);
}
}
}
else
{
for (int i_0 = 0; i_0 < numBodies; i_0++)
{
if (b.Equals(items[i_0]))
{
RemoveBody(i_0);
return(true);
}
}
}
return(false);
}
public PBody Polygon(bool fix, float[] xs, float[] ys, int num,
float angle, float density)
{
if (num < 3)
{
return(null);
}
if (xs.Length != num)
{
xs = CollectionUtils.CopyOf(xs, num);
}
if (ys.Length != num)
{
ys = CollectionUtils.CopyOf(ys, num);
}
for (int i = 0; i < num; i++)
{
xs[i] /= scale;
ys[i] /= scale;
}
PConcavePolygonShape shape = new PConcavePolygonShape(xs, ys, density);
PBody body = new PBody(angle, fix, new PShape[] { shape });
return(body);
}
public PBody AddBox(bool fix, float x, float y, float w, float h,
float angle, float density)
{
PBody body = Box(fix, x, y, w, h, angle, density);
world.AddBody(body);
return(body);
}
public PBody AddPolygon(bool fix, float[] xs, float[] ys, int num,
float angle, float density)
{
PBody body = Polygon(fix, xs, ys, num, angle, density);
world.AddBody(body);
return(body);
}
public PBody AddCircle(bool fix, float x, float y, float r, float angle,
float density)
{
PBody body = Circle(fix, x, y, r, angle, density);
world.AddBody(body);
return(body);
}
public static PTransformer CalcEffectiveMass(PBody b, Vector2f r)
{
PTransformer mass = new PTransformer(b.invM + b.invI * r.y * r.y,
-b.invI * r.x * r.y, -b.invI * r.x * r.y, b.invM + b.invI * r.x
* r.x);
mass.InvertEqual();
return(mass);
}
public PBody Polygon(bool fix, Polygon p, float angle, float density)
{
PPolygon tmp = p.GetPPolygon(this.scale);
PConcavePolygonShape shape = new PConcavePolygonShape(tmp.xs, tmp.ys,
density);
PBody body = new PBody(angle, fix, new PShape[] { shape });
return(body);
}
public PDragJoint(PBody b_0, float px, float py) {
this.b = b_0;
dragPoint = new Vector2f(px, py);
localAnchor = new Vector2f(px - b_0.pos.x, py - b_0.pos.y);
b_0.mAng.Transpose().MulEqual(localAnchor);
anchor = b_0.mAng.Mul(localAnchor);
anchor.AddLocal(b_0.pos);
type = Physics.PJointType.DRAG_JOINT;
mass = new PTransformer();
}
public static float CalcEffectiveMass(PBody b1, PBody b2, Vector2f r1,
Vector2f r2, Vector2f normal)
{
float rn1 = normal.Dot(r1);
float rn2 = normal.Dot(r2);
return(1.0F / (b1.invM + b2.invM + b1.invI
* ((r1.x * r1.x + r1.y * r1.y) - rn1 * rn1) + b2.invI
* ((r2.x * r2.x + r2.y * r2.y) - rn2 * rn2)));
}
public PDragJoint(PBody b_0, float px, float py)
{
this.b = b_0;
dragPoint = new Vector2f(px, py);
localAnchor = new Vector2f(px - b_0.pos.x, py - b_0.pos.y);
b_0.mAng.Transpose().MulEqual(localAnchor);
anchor = b_0.mAng.Mul(localAnchor);
anchor.AddLocal(b_0.pos);
type = Physics.PJointType.DRAG_JOINT;
mass = new PTransformer();
}
public void AddBody(PBody b) {
if (numBodies + 1 >= bodies.Length) {
bodies = (PBody[]) CollectionUtils
.CopyOf(bodies, bodies.Length * 2);
}
b.w = this;
for (int i = 0; i < b.numShapes; i++) {
AddShape(b.shapes[i]);
}
bodies[numBodies] = b;
numBodies++;
}
public static PTransformer CalcEffectiveMass(PBody b1, PBody b2,
Vector2f r1, Vector2f r2)
{
PTransformer mass = new PTransformer(b1.invM + b2.invM + b1.invI * r1.y
* r1.y + b2.invI * r2.y * r2.y, -b1.invI * r1.x * r1.y
- b2.invI * r2.x * r2.y, -b1.invI * r1.x * r1.y - b2.invI
* r2.x * r2.y, b1.invM + b2.invM + b1.invI * r1.x * r1.x
+ b2.invI * r2.x * r2.x);
mass.InvertEqual();
return(mass);
}
public void Step(float dt)
{
long st = (System.DateTime.Now.Ticks * 100);
for (int i = 0; i < numBodies; i++)
{
if (bodies[i].rem)
{
RemoveBody(i);
i--;
}
else
{
bodies[i].Update();
if (!bodies[i].fix)
{
PBody b = bodies[i];
b.vel.x += gravity.x * dt;
b.vel.y += gravity.y * dt;
}
}
}
for (int i_0 = 0; i_0 < numShapes; i_0++)
{
if (shapes[i_0]._rem)
{
RemoveShape(i_0);
i_0--;
}
}
for (int i_1 = 0; i_1 < numJoints; i_1++)
{
if (joints[i_1].rem)
{
RemoveJoint(i_1);
i_1--;
}
else
{
joints[i_1].Update();
}
}
long en = (System.DateTime.Now.Ticks * 100);
positionUpdateTime = en - st;
Collide(en);
Solve(dt);
long totalEn = (System.DateTime.Now.Ticks * 100);
totalStepTime = totalEn - st;
}
public static Vector2f CalcRelativeVelocity(PBody b1, PBody b2,
Vector2f r1, Vector2f r2)
{
Vector2f relVel = b1.vel.Clone();
relVel.x -= b2.vel.x;
relVel.y -= b2.vel.y;
relVel.x += -b1.angVel * r1.y;
relVel.y += b1.angVel * r1.x;
relVel.x -= -b2.angVel * r2.y;
relVel.y -= b2.angVel * r2.x;
return(relVel);
}
public PBody Box(bool fix, float x, float y, float w, float h,
float angle, float density) {
PBody body = null;
if (!isCenterPos) {
body = new PBody(angle, fix, new PShape[] { new PBoxShape(
(x + w / 2) / scale, (y + h / 2) / scale, w / scale, h
/ scale, angle, density) });
} else {
body = new PBody(angle, fix,
new PShape[] { new PBoxShape(x / scale, y / scale, w
/ scale, h / scale, angle, density) });
}
return body;
}
public PRodJoint(PBody b1_0, PBody b2_1, float rel1x, float rel1y, float rel2x,
float rel2y, float distance) {
this.b1 = b1_0;
this.b2 = b2_1;
localAnchor1 = new Vector2f(rel1x, rel1y);
localAnchor2 = new Vector2f(rel2x, rel2y);
b1_0.mAng.Transpose().MulEqual(localAnchor1);
b2_1.mAng.Transpose().MulEqual(localAnchor2);
dist = distance;
anchor1 = b1_0.mAng.Mul(localAnchor1).Add(b1_0.pos);
anchor2 = b2_1.mAng.Mul(localAnchor2).Add(b2_1.pos);
normal = anchor1.Sub(anchor2);
normal.Normalize();
type = Physics.PJointType.ROD_JOINT;
}
public void AddBody(PBody b)
{
if (numBodies + 1 >= bodies.Length)
{
bodies = (PBody[])CollectionUtils
.CopyOf(bodies, bodies.Length * 2);
}
b.w = this;
for (int i = 0; i < b.numShapes; i++)
{
AddShape(b.shapes[i]);
}
bodies[numBodies] = b;
numBodies++;
}
public PRodJoint(PBody b1_0, PBody b2_1, float rel1x, float rel1y, float rel2x,
float rel2y, float distance)
{
this.b1 = b1_0;
this.b2 = b2_1;
localAnchor1 = new Vector2f(rel1x, rel1y);
localAnchor2 = new Vector2f(rel2x, rel2y);
b1_0.mAng.Transpose().MulEqual(localAnchor1);
b2_1.mAng.Transpose().MulEqual(localAnchor2);
dist = distance;
anchor1 = b1_0.mAng.Mul(localAnchor1).Add(b1_0.pos);
anchor2 = b2_1.mAng.Mul(localAnchor2).Add(b2_1.pos);
normal = anchor1.Sub(anchor2);
normal.Normalize();
type = Physics.PJointType.ROD_JOINT;
}
public PHingeJoint(PBody b1_0, PBody b2_1, float rel1x, float rel1y,
float rel2x, float rel2y) {
this.b1 = b1_0;
this.b2 = b2_1;
localAngle = b2_1.ang - b1_0.ang;
localAnchor1 = new Vector2f(rel1x, rel1y);
localAnchor2 = new Vector2f(rel2x, rel2y);
b1_0.mAng.Transpose().MulEqual(localAnchor1);
b2_1.mAng.Transpose().MulEqual(localAnchor2);
anchor1 = b1_0.mAng.Mul(localAnchor1);
anchor1.AddLocal(b1_0.pos);
anchor2 = b2_1.mAng.Mul(localAnchor2);
anchor2.AddLocal(b2_1.pos);
type = Physics.PJointType.HINGE_JOINT;
mass = new PTransformer();
impulse = new Vector2f();
}
public PBody Circle(bool fix, float x, float y, float r, float angle,
float density)
{
PBody body = null;
if (!isCenterPos)
{
body = new PBody(angle, fix, new PShape[] { new PCircleShape(x
/ scale, y / scale, r / scale, angle, density) });
}
else
{
body = new PBody(angle, fix, new PShape[] { new PCircleShape(x
/ scale, y / scale, r / scale, angle, density) });
}
return(body);
}
public PHingeJoint(PBody b1_0, PBody b2_1, float rel1x, float rel1y,
float rel2x, float rel2y)
{
this.b1 = b1_0;
this.b2 = b2_1;
localAngle = b2_1.ang - b1_0.ang;
localAnchor1 = new Vector2f(rel1x, rel1y);
localAnchor2 = new Vector2f(rel2x, rel2y);
b1_0.mAng.Transpose().MulEqual(localAnchor1);
b2_1.mAng.Transpose().MulEqual(localAnchor2);
anchor1 = b1_0.mAng.Mul(localAnchor1);
anchor1.AddLocal(b1_0.pos);
anchor2 = b2_1.mAng.Mul(localAnchor2);
anchor2.AddLocal(b2_1.pos);
type = Physics.PJointType.HINGE_JOINT;
mass = new PTransformer();
impulse = new Vector2f();
}
public PSpringJoint(PBody b1_0, PBody i_2, float rel1x, float rel1y,
float rel2x, float rel2y, float distance, float strength,
float damping) {
this.b1 = b1_0;
this.b2 = i_2;
str = strength;
damp = damping;
localAnchor1 = new Vector2f(rel1x, rel1y);
localAnchor2 = new Vector2f(rel2x, rel2y);
b1_0.mAng.Transpose().MulEqual(localAnchor1);
i_2.mAng.Transpose().MulEqual(localAnchor2);
dist = distance;
anchor1 = b1_0.mAng.Mul(localAnchor1).Add(b1_0.pos);
anchor2 = i_2.mAng.Mul(localAnchor2).Add(i_2.pos);
normal = anchor1.Sub(anchor2);
normal.Normalize();
type = Physics.PJointType.SPRING_JOINT;
}
public PBody Box(bool fix, float x, float y, float w, float h,
float angle, float density)
{
PBody body = null;
if (!isCenterPos)
{
body = new PBody(angle, fix, new PShape[] { new PBoxShape(
(x + w / 2) / scale, (y + h / 2) / scale, w / scale, h
/ scale, angle, density) });
}
else
{
body = new PBody(angle, fix,
new PShape[] { new PBoxShape(x / scale, y / scale, w
/ scale, h / scale, angle, density) });
}
return(body);
}
public bool RemoveBody(PBody b, bool identity) {
object[] items = this.bodies;
if (identity || b == null) {
for (int i = 0; i < numBodies; i++) {
if (items[i] == (object) b) {
RemoveBody(i);
return true;
}
}
} else {
for (int i_0 = 0; i_0 < numBodies; i_0++) {
if (b.Equals(items[i_0])) {
RemoveBody(i_0);
return true;
}
}
}
return false;
}
public PSpringJoint(PBody b1_0, PBody i_2, float rel1x, float rel1y,
float rel2x, float rel2y, float distance, float strength,
float damping)
{
this.b1 = b1_0;
this.b2 = i_2;
str = strength;
damp = damping;
localAnchor1 = new Vector2f(rel1x, rel1y);
localAnchor2 = new Vector2f(rel2x, rel2y);
b1_0.mAng.Transpose().MulEqual(localAnchor1);
i_2.mAng.Transpose().MulEqual(localAnchor2);
dist = distance;
anchor1 = b1_0.mAng.Mul(localAnchor1).Add(b1_0.pos);
anchor2 = i_2.mAng.Mul(localAnchor2).Add(i_2.pos);
normal = anchor1.Sub(anchor2);
normal.Normalize();
type = Physics.PJointType.SPRING_JOINT;
}
public PBody BindPhysics(PBody body, SpriteBatchObject o)
{
if (usePhysics)
{
body.SetTag(o);
_manager.AddBody(body);
CollectionUtils.Put(_Bodys, o, body);
return body;
}
else
{
throw new RuntimeException("You do not set the physics engine !");
}
}
public void SetSouthBody(PBody southBody_0)
{
this.southBody = southBody_0;
}
public bool RemoveBody(PBody b) {
return RemoveBody(b, false);
}
public bool RemoveBody(PBody b)
{
return(RemoveBody(b, false));
}
public static float CalcEffectiveMass(PBody b1, PBody b2, Vector2f r1,
Vector2f r2, Vector2f normal) {
float rn1 = normal.Dot(r1);
float rn2 = normal.Dot(r2);
return 1.0F / (b1.invM + b2.invM + b1.invI
* ((r1.x * r1.x + r1.y * r1.y) - rn1 * rn1) + b2.invI
* ((r2.x * r2.x + r2.y * r2.y) - rn2 * rn2));
}
public static Vector2f CalcRelativeVelocity(PBody b1, PBody b2,
Vector2f r1, Vector2f r2) {
Vector2f relVel = b1.vel.Clone();
relVel.x -= b2.vel.x;
relVel.y -= b2.vel.y;
relVel.x += -b1.angVel * r1.y;
relVel.y += b1.angVel * r1.x;
relVel.x -= -b2.angVel * r2.y;
relVel.y -= b2.angVel * r2.x;
return relVel;
}
private void Solve(float dt)
{
long st = (System.DateTime.Now.Ticks * 100);
for (int i = 0; i < numSolvers; i++)
{
solvers[i].PreSolve();
}
for (int i_0 = 0; i_0 < numJoints; i_0++)
{
joints[i_0].PreSolve(dt);
}
for (int j = 0; j < iterations; j++)
{
for (int i_1 = 0; i_1 < numJoints; i_1++)
{
joints[i_1].SolveVelocity(dt);
}
for (int i_2 = 0; i_2 < numSolvers; i_2++)
{
solvers[i_2].SolveVelocity();
}
}
long en = (System.DateTime.Now.Ticks * 100);
collisionSolveTime = en - st;
st = (System.DateTime.Now.Ticks * 100);
for (int i_3 = 0; i_3 < numBodies; i_3++)
{
if (!bodies[i_3].fix)
{
PBody b = bodies[i_3];
b.correctVel.x = b.vel.x * dt;
b.correctVel.y = b.vel.y * dt;
b.correctAngVel = b.angVel * dt;
}
}
en = (System.DateTime.Now.Ticks * 100);
positionUpdateTime += en - st;
st = (System.DateTime.Now.Ticks * 100);
for (int j_4 = 0; j_4 < iterations; j_4++)
{
for (int i_5 = 0; i_5 < numJoints; i_5++)
{
joints[i_5].SolvePosition();
}
for (int i_6 = 0; i_6 < numSolvers; i_6++)
{
solvers[i_6].SolvePosition();
}
}
en = (System.DateTime.Now.Ticks * 100);
collisionSolveTime += en - st;
st = (System.DateTime.Now.Ticks * 100);
for (int i_7 = 0; i_7 < numBodies; i_7++)
{
PBody b_8 = bodies[i_7];
if (b_8.fix)
{
b_8.angVel = 0.0F;
b_8.vel.Set(0.0F, 0.0F);
}
else
{
b_8.pos.x += b_8.correctVel.x;
b_8.pos.y += b_8.correctVel.y;
b_8.ang += b_8.correctAngVel;
}
b_8.Update();
}
for (int i_9 = 0; i_9 < numJoints; i_9++)
{
joints[i_9].Update();
}
en = (System.DateTime.Now.Ticks * 100);
positionUpdateTime += en - st;
}
public void SetWestBody(PBody westBody_0)
{
this.westBody = westBody_0;
}
public void SetWestBody(PBody westBody_0) {
this.westBody = westBody_0;
}
public PBody Polygon(bool fix, float[] xs, float[] ys, int num,
float angle, float density) {
if (num < 3) {
return null;
}
if (xs.Length != num) {
xs = CollectionUtils.CopyOf(xs, num);
}
if (ys.Length != num) {
ys = CollectionUtils.CopyOf(ys, num);
}
for (int i = 0; i < num; i++) {
xs[i] /= scale;
ys[i] /= scale;
}
PConcavePolygonShape shape = new PConcavePolygonShape(xs, ys, density);
PBody body = new PBody(angle, fix, new PShape[] { shape });
return body;
}
public void SetSouthBody(PBody southBody_0) {
this.southBody = southBody_0;
}
public void SetNorthBody(PBody northBody_0) {
this.northBody = northBody_0;
}
public void SetEastBody(PBody eastBody_0) {
this.eastBody = eastBody_0;
}
public void SetNorthBody(PBody northBody_0)
{
this.northBody = northBody_0;
}
public PBody Shape(bool fix, PShape shape, float angle, float density) {
PBody body = new PBody(angle, fix, new PShape[] { shape });
return body;
}
public void SetEastBody(PBody eastBody_0)
{
this.eastBody = eastBody_0;
}
public PBody AddBody(PBody body) {
world.AddBody(body);
return body;
}
public static PTransformer CalcEffectiveMass(PBody b1, PBody b2,
Vector2f r1, Vector2f r2) {
PTransformer mass = new PTransformer(b1.invM + b2.invM + b1.invI * r1.y
* r1.y + b2.invI * r2.y * r2.y, -b1.invI * r1.x * r1.y
- b2.invI * r2.x * r2.y, -b1.invI * r1.x * r1.y - b2.invI
* r2.x * r2.y, b1.invM + b2.invM + b1.invI * r1.x * r1.x
+ b2.invI * r2.x * r2.x);
mass.InvertEqual();
return mass;
}
public PBody AddBody(PBody body)
{
world.AddBody(body);
return(body);
}
public static PTransformer CalcEffectiveMass(PBody b, Vector2f r) {
PTransformer mass = new PTransformer(b.invM + b.invI * r.y * r.y,
-b.invI * r.x * r.y, -b.invI * r.x * r.y, b.invM + b.invI * r.x
* r.x);
mass.InvertEqual();
return mass;
}
public PBody Circle(bool fix, float x, float y, float r, float angle,
float density) {
PBody body = null;
if (!isCenterPos) {
body = new PBody(angle, fix, new PShape[] { new PCircleShape(x
/ scale, y / scale, r / scale, angle, density) });
} else {
body = new PBody(angle, fix, new PShape[] { new PCircleShape(x
/ scale, y / scale, r / scale, angle, density) });
}
return body;
}
public PBody Polygon(bool fix, Polygon p, float angle, float density) {
PPolygon tmp = p.GetPPolygon(this.scale);
PConcavePolygonShape shape = new PConcavePolygonShape(tmp.xs, tmp.ys,
density);
PBody body = new PBody(angle, fix, new PShape[] { shape });
return body;
}
public PBody Shape(bool fix, PShape shape, float angle, float density)
{
PBody body = new PBody(angle, fix, new PShape[] { shape });
return(body);
}
