Beispiel #1
0
 public void AddEqual(PTransformer m)
 {
     e00 += m.e00;
     e01 += m.e01;
     e10 += m.e10;
     e11 += m.e11;
 }
Beispiel #2
0
		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);
		}
Beispiel #3
0
 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();
 }
Beispiel #4
0
		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);
		}
Beispiel #5
0
        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);
        }
Beispiel #6
0
        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));
        }
Beispiel #7
0
        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);
        }
Beispiel #8
0
		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();
		}
Beispiel #9
0
		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;
		}
Beispiel #10
0
 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();
 }
Beispiel #11
0
        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);
        }
Beispiel #12
0
        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);
        }
Beispiel #13
0
 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);
 }
Beispiel #14
0
		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);
		}
Beispiel #15
0
		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));
			}
		}
Beispiel #16
0
		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();
		}
Beispiel #17
0
        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);
        }
Beispiel #18
0
 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));
     }
 }
Beispiel #19
0
 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();
 }
Beispiel #20
0
        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);
        }
Beispiel #21
0
        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);
        }
Beispiel #22
0
        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);
        }
Beispiel #23
0
        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);
            }
        }
Beispiel #24
0
		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();
		}
Beispiel #25
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);
        }
Beispiel #26
0
        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);
        }
Beispiel #27
0
		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);
		}
Beispiel #28
0
		public void AddEqual(PTransformer m) {
			e00 += m.e00;
			e01 += m.e01;
			e10 += m.e10;
			e11 += m.e11;
		}
Beispiel #29
0
		public PTransformer Add(PTransformer m) {
	
			return new PTransformer(e00 + m.e00, e01 + m.e01, e10 + m.e10, e11
					+ m.e11);
		}
Beispiel #30
0
		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;
		}
Beispiel #31
0
		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;
		}
Beispiel #32
0
 public PTransformer Add(PTransformer m)
 {
     return(new PTransformer(e00 + m.e00, e01 + m.e01, e10 + m.e10, e11
                             + m.e11));
 }