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 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;
}
internal override void PreSolve(float dt) {
relAnchor1 = b1.mAng.Mul(localAnchor1);
relAnchor2 = b2.mAng.Mul(localAnchor2);
anchor1.Set(relAnchor1.x + b1.pos.x, relAnchor1.y + b1.pos.y);
anchor2.Set(relAnchor2.x + b2.pos.x, relAnchor2.y + b2.pos.y);
normal = anchor2.Sub(anchor1);
length = normal.Length();
normal.Normalize();
mass = PTransformer.CalcEffectiveMass(b1, b2, relAnchor1, relAnchor2,
normal);
b1.ApplyImpulse(normal.x * norI, normal.y * norI, anchor1.x, anchor1.y);
b2.ApplyImpulse(normal.x * -norI, normal.y * -norI, anchor2.x,
anchor2.y);
}
public virtual int Collide(PShape s1, PShape s2, PContact[] cs) {
if (s1._type != PShapeType.CIRCLE_SHAPE
|| s2._type != PShapeType.CONVEX_SHAPE
&& s2._type != PShapeType.BOX_SHAPE) {
return 0;
}
PCircleShape c1 = (PCircleShape) s1;
PConvexPolygonShape p1 = (PConvexPolygonShape) s2;
float distance = -1F;
int edgeNumber = -1;
Vector2f[] vers = p1.vers;
int numVers = p1.numVertices;
Vector2f normal = new Vector2f();
Vector2f edgeNormal = new Vector2f();
Vector2f a = new Vector2f();
Vector2f b = new Vector2f();
int num = 0;
for (int i = 0; i < numVers; i++) {
a.Set(c1._pos.x - vers[i].x, c1._pos.y - vers[i].y);
distance = a.Length();
distance -= c1.rad;
if (distance <= 0.0F) {
PContact c = new PContact();
c.overlap = distance;
a.Normalize();
c.normal.Set(a.x, a.y);
c.pos.Set(vers[i].x, vers[i].y);
cs[num] = c;
if (++num == 2) {
return num;
}
}
}
if (num > 0) {
return num;
}
for (int i_0 = 0; i_0 < numVers; i_0++) {
Vector2f ver = vers[i_0];
Vector2f nextVer = vers[(i_0 + 1) % numVers];
float edgeX = nextVer.x - ver.x;
float edgeY = nextVer.y - ver.y;
edgeNormal.Set(edgeY, -edgeX);
edgeNormal.Normalize();
a.Set(c1._pos.x - ver.x, c1._pos.y - ver.y);
b.Set(c1._pos.x - nextVer.x, c1._pos.y - nextVer.y);
if ((a.x * edgeX + a.y * edgeY) * (b.x * edgeX + b.y * edgeY) <= 0.0F) {
float edgeLen = (float) System.Math.Sqrt(edgeX * edgeX + edgeY * edgeY);
float distanceToEdge = System.Math.Abs(a.x * edgeY - a.y * edgeX)
/ edgeLen;
if (distanceToEdge <= c1.rad) {
distanceToEdge -= c1.rad;
if (distance > distanceToEdge || distance == -1F) {
edgeNumber = i_0;
distance = distanceToEdge;
normal.Set(edgeNormal.x, edgeNormal.y);
}
}
}
}
if (edgeNumber > -1) {
PContact c_1 = new PContact();
c_1.overlap = distance;
c_1.normal = normal;
c_1.pos = c1._pos.Sub(normal.Mul(c1.rad));
cs[0] = c_1;
return 1;
}
bool hit = true;
for (int i_2 = 0; i_2 < numVers; i_2++) {
Vector2f ver = vers[i_2];
Vector2f nextVer = vers[(i_2 + 1) % numVers];
float v1x = nextVer.x - ver.x;
float v1y = nextVer.y - ver.y;
float v2x = c1._pos.x - ver.x;
float v2y = c1._pos.y - ver.y;
if (v1x * v2y - v1y * v2x >= 0.0F) {
continue;
}
hit = false;
break;
}
if (hit) {
distance = 1.0F;
normal = new Vector2f();
for (int i = 0; i < numVers; i++) {
Vector2f ver = vers[i];
Vector2f nextVer = vers[(i + 1) % numVers];
a.Set(nextVer.x - ver.x, nextVer.y - ver.y);
a.Normalize();
float d = c1._pos.Sub(ver).Cross(a);
if (d < 0.0F && (distance == 1.0F || distance < d)) {
distance = d;
normal.Set(a.y, -a.x);
}
}
if (distance != 1.0F) {
PContact c = new PContact();
c.normal.Set(normal.x, normal.y);
c.pos.Set(c1._pos.x, c1._pos.y);
c.overlap = distance;
cs[0] = c;
return 1;
}
}
return 0;
}
internal override void PreSolve(float i_0) {
relAnchor1 = b1.mAng.Mul(localAnchor1);
relAnchor2 = b2.mAng.Mul(localAnchor2);
anchor1.Set(relAnchor1.x + b1.pos.x, relAnchor1.y + b1.pos.y);
anchor2.Set(relAnchor2.x + b2.pos.x, relAnchor2.y + b2.pos.y);
normal = anchor2.Sub(anchor1);
float over = dist - normal.Length();
normal.Normalize();
mass = PTransformer.CalcEffectiveMass(b1, b2, relAnchor1, relAnchor2,
normal);
float k = mass * 1000F * str;
force = -over * k;
force += PTransformer.CalcRelativeVelocity(b1, b2, relAnchor1,
relAnchor2).Dot(normal)
* damp * -(float) System.Math.Sqrt(k * mass) * 2.0F;
force *= i_0;
b1.ApplyImpulse(normal.x * force, normal.y * force, anchor1.x,
anchor1.y);
b2.ApplyImpulse(normal.x * -force, normal.y * -force, anchor2.x,
anchor2.y);
}
