public void AddEqual(PTransformer m) { e00 += m.e00; e01 += m.e01; e10 += m.e10; e11 += m.e11; }
public PTransformer Mul(PTransformer m) { float t11 = e00 * m.e00 + e01 * m.e10; float t12 = e00 * m.e01 + e01 * m.e11; float t13 = e10 * m.e00 + e11 * m.e10; float t14 = e10 * m.e01 + e11 * m.e11; return new PTransformer(t11, t12, t13, t14); }
public PBody(float angle, bool fixate, PShape[] ss) { pos = new Vector2f(); vel = new Vector2f(); correctVel = new Vector2f(); mAng = new PTransformer(); aabb = new AABB(); ang = angle; numShapes = ss.Length; shapes = new PShape[1024]; for (int i_0 = 0; i_0 < numShapes; i_0++) { shapes[i_0] = ss[i_0]; shapes[i_0]._parent = this; if (shapes[i_0]._type == PShapeType.CONCAVE_SHAPE) { PConcavePolygonShape cp = (PConcavePolygonShape)shapes[i_0]; for (int j = 0; j < cp.numConvexes; j++) { cp.convexes[j]._parent = this; } } } fix = fixate; CalcMassData(); }
public void MulEqual(PTransformer m) { float t11 = e00 * m.e00 + e01 * m.e10; float t12 = e00 * m.e01 + e01 * m.e11; float t13 = e10 * m.e00 + e11 * m.e10; float t14 = e10 * m.e01 + e11 * m.e11; Set(t11, t12, t13, t14); }
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 PTransformer Mul(PTransformer m) { float t11 = e00 * m.e00 + e01 * m.e10; float t12 = e00 * m.e01 + e01 * m.e11; float t13 = e10 * m.e00 + e11 * m.e10; float t14 = e10 * m.e01 + e11 * m.e11; return(new PTransformer(t11, t12, t13, t14)); }
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 PTransformer Invert() { float d = e00 * e11 - e10 * e01; if (d != 0.0F) d = 1.0F / d; else return new PTransformer(); PTransformer ret = new PTransformer(d * e11, -d * e01, -d * e10, d * e00); return ret; }
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); }
internal override void SolveVelocity(float dt) { float rn = normal.Dot(PTransformer.CalcRelativeVelocity(b1, b2, relAnchor1, relAnchor2)); float impulse = -mass * rn; norI += impulse; float forceX = normal.x * impulse; float forceY = normal.y * impulse; b1.ApplyImpulse(forceX, forceY, anchor1.x, anchor1.y); b2.ApplyImpulse(-forceX, -forceY, anchor2.x, anchor2.y); }
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); }
internal override void PreSolve(float dt) { relAnchor = b.mAng.Mul(localAnchor); anchor.Set(relAnchor.x + b.pos.x, relAnchor.y + b.pos.y); mass = PTransformer.CalcEffectiveMass(b, relAnchor); Vector2f f = anchor.Sub(dragPoint); float k = b.m; f.MulLocal(-k * 20F); Vector2f relVel = b.vel.Clone(); relVel.x += -b.angVel * relAnchor.y; relVel.y += b.angVel * relAnchor.x; relVel.MulLocal((float) System.Math.Sqrt(k * 20F * k)); f.SubLocal(relVel); f.MulLocal(dt); mass.MulEqual(f); b.ApplyImpulse(f.x, f.y, anchor.x, anchor.y); }
public PShape(bool randColor) { _fric = 0.5F; _rest = 0.5F; _localPos = new Vector2f(); _pos = new Vector2f(); _mAng = new PTransformer(); _aabb = new AABB(); _sapAABB = new PSortableAABB(); _type = Physics.PShapeType.NULL_SHAPE; _rnd = randColor; if (randColor) { SetColor((int) (MathUtils.Random() * 160F + 96F), (int) (MathUtils.Random() * 160F + 96F), (int) (MathUtils.Random() * 160F + 96F)); } }
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 PTransformer Invert() { float d = e00 * e11 - e10 * e01; if (d != 0.0F) { d = 1.0F / d; } else { return(new PTransformer()); } PTransformer ret = new PTransformer(d * e11, -d * e01, -d * e10, d * e00); return(ret); }
public PShape(bool randColor) { _fric = 0.5F; _rest = 0.5F; _localPos = new Vector2f(); _pos = new Vector2f(); _mAng = new PTransformer(); _aabb = new AABB(); _sapAABB = new PSortableAABB(); _type = Physics.PShapeType.NULL_SHAPE; _rnd = randColor; if (randColor) { SetColor((int)(MathUtils.Random() * 160F + 96F), (int)(MathUtils.Random() * 160F + 96F), (int)(MathUtils.Random() * 160F + 96F)); } }
internal override void PreSolve(float dt) { relAnchor = b.mAng.Mul(localAnchor); anchor.Set(relAnchor.x + b.pos.x, relAnchor.y + b.pos.y); mass = PTransformer.CalcEffectiveMass(b, relAnchor); Vector2f f = anchor.Sub(dragPoint); float k = b.m; f.MulLocal(-k * 20F); Vector2f relVel = b.vel.Clone(); relVel.x += -b.angVel * relAnchor.y; relVel.y += b.angVel * relAnchor.x; relVel.MulLocal((float)System.Math.Sqrt(k * 20F * k)); f.SubLocal(relVel); f.MulLocal(dt); mass.MulEqual(f); b.ApplyImpulse(f.x, f.y, anchor.x, anchor.y); }
internal override void SolvePosition() { if (enableLimit && limitState != 0) { float over = b2.ang - b1.ang - localAngle; if (over < minAngle) { over += 0.008F; over = ((over - minAngle) + b2.correctAngVel) - b1.correctAngVel; float torque = over * 0.2F * angM; float subAngleImpulse = angI; angI = MathUtils.Min(angI + torque, 0.0F); torque = angI - subAngleImpulse; b1.PositionCorrection(torque); b2.PositionCorrection(-torque); } if (over > maxAngle) { over -= 0.008F; over = ((over - maxAngle) + b2.correctAngVel) - b1.correctAngVel; float torque_0 = over * 0.2F * angM; float subAngleImpulse_1 = angI; angI = MathUtils.Max(angI + torque_0, 0.0F); torque_0 = angI - subAngleImpulse_1; b1.PositionCorrection(torque_0); b2.PositionCorrection(-torque_0); } } Vector2f force = anchor2.Sub(anchor1); force.SubLocal(PTransformer.CalcRelativeCorrectVelocity(b1, b2, relAnchor1, relAnchor2)); float length = force.Length(); force.Normalize(); force.MulLocal(System.Math.Max(length * 0.2F - 0.002F, 0.0F)); mass.MulEqual(force); b1.PositionCorrection(force.x, force.y, anchor1.x, anchor1.y); b2.PositionCorrection(-force.x, -force.y, anchor2.x, anchor2.y); }
internal override void SolvePosition() { float rvn = normal.Dot(PTransformer.CalcRelativeCorrectVelocity(b1, b2, relAnchor1, relAnchor2)); float impulse = -mass * ((dist - length) + rvn) * 0.2F; if (impulse > 0.0F) { impulse = Max(impulse - 0.002F, 0.0F); } else if (impulse < 0.0F) { impulse = Min(impulse + 0.002F, 0.0F); } float forceX = normal.x * impulse; float forceY = normal.y * impulse; b1.PositionCorrection(forceX, forceY, anchor1.x, anchor1.y); b2.PositionCorrection(-forceX, -forceY, anchor2.x, anchor2.y); }
internal override void SolveVelocity(float dt) { Vector2f relVel = PTransformer.CalcRelativeVelocity(b1, b2, relAnchor1, relAnchor2); Vector2f force = mass.Mul(relVel).Negate(); impulse.AddLocal(force); b1.ApplyImpulse(force.x, force.y, anchor1.x, anchor1.y); b2.ApplyImpulse(-force.x, -force.y, anchor2.x, anchor2.y); if (enableMotor) { float angRelVel = b2.angVel - b1.angVel - motorSpeed; float torque = angM * angRelVel; float subMotorI = motI; motI = MathUtils.Clamp(motI + torque, -motorTorque * dt, motorTorque * dt); torque = motI - subMotorI; b1.ApplyTorque(torque); b2.ApplyTorque(-torque); } if (enableLimit && limitState != 0) { float angRelVel_0 = b2.angVel - b1.angVel - targetAngleSpeed; float torque_1 = angM * angRelVel_0; float subLimitI = limI; if (limitState == -1) { limI = MathUtils.Min(limI + torque_1, 0.0F); } else { if (limitState == 1) { limI = MathUtils.Max(limI + torque_1, 0.0F); } } torque_1 = limI - subLimitI; b1.ApplyTorque(torque_1); b2.ApplyTorque(-torque_1); } }
public PBody(float angle, bool fixate, PShape[] ss) { pos = new Vector2f(); vel = new Vector2f(); correctVel = new Vector2f(); mAng = new PTransformer(); aabb = new AABB(); ang = angle; numShapes = ss.Length; shapes = new PShape[1024]; for (int i_0 = 0; i_0 < numShapes; i_0++) { shapes[i_0] = ss[i_0]; shapes[i_0]._parent = this; if (shapes[i_0]._type == PShapeType.CONCAVE_SHAPE) { PConcavePolygonShape cp = (PConcavePolygonShape) shapes[i_0]; for (int j = 0; j < cp.numConvexes; j++) { cp.convexes[j]._parent = this; } } } fix = fixate; CalcMassData(); }
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); }
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); mass = PTransformer.CalcEffectiveMass(b1, b2, relAnchor1, relAnchor2); angM = 1.0F / (b1.invI + b2.invI); float ang = b2.ang - b1.ang - localAngle; if (!enableMotor) { motI = 0.0F; } if (enableLimit) { if (ang < minAngle) { if (limitState != -1) { limI = 0.0F; } limitState = -1; if (b2.angVel - b1.angVel < 0.0F) { targetAngleSpeed = (b2.angVel - b1.angVel) * -rest; } else { targetAngleSpeed = 0.0F; } } else if (ang > maxAngle) { if (limitState != 1) { limI = 0.0F; } limitState = 1; if (b2.angVel - b1.angVel > 0.0F) { targetAngleSpeed = (b2.angVel - b1.angVel) * -rest; } else { targetAngleSpeed = 0.0F; } } else { limI = limitState = 0; } } else { limI = limitState = 0; } angI = 0.0F; b1.ApplyImpulse(impulse.x, impulse.y, anchor1.x, anchor1.y); b2.ApplyImpulse(-impulse.x, -impulse.y, anchor2.x, anchor2.y); b1.ApplyTorque(motI + limI); b2.ApplyTorque(-motI - limI); }
public PTransformer Add(PTransformer m) { return new PTransformer(e00 + m.e00, e01 + m.e01, e10 + m.e10, e11 + m.e11); }
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 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 PTransformer Add(PTransformer m) { return(new PTransformer(e00 + m.e00, e01 + m.e01, e10 + m.e10, e11 + m.e11)); }