public Motor(AbstractParticle attach, float radius, int color) { wheel = new WheelParticle(attach.PX, attach.PY - .01f, radius); wheel.SetStyle(0, 0, Color.FromArgb(255 / 2, 255, 0, 0).ToArgb()); SpringConstraint axle = new SpringConstraint(wheel, attach); _rimA = new CircleParticle(0, 0, 2, true); _rimB = new CircleParticle(0, 0, 2, true); _rimC = new CircleParticle(0, 0, 2, true); wheel.Collidable = false; _rimA.Collidable = false; _rimB.Collidable = false; _rimC.Collidable = false; this.Particles.Add(_rimA); this.Particles.Add(_rimB); this.Particles.Add(_rimC); this.Particles.Add(wheel); this.Constraints.Add(axle); this.color = color; this.radius = radius; // run it once to make sure the rim particles are in the right place run(); }
public static void test(AbstractParticle objA, AbstractParticle objB) { if (objA.Fixed && objB.Fixed) return; if (objA.MultiSample == 0 && objB.MultiSample == 0) { normVsNorm(objA, objB); } else if (objA.MultiSample > 0 && objB.MultiSample == 0) { sampVsNorm(objA, objB); } else if (objB.MultiSample > 0 && objA.MultiSample == 0) { sampVsNorm(objB, objA); } else if (objA.MultiSample == objB.MultiSample) { sampVsSamp(objA, objB); } else { normVsNorm(objA, objB); } }
public SpringConstraint(AbstractParticle p1, AbstractParticle p2, float stiffness, bool collidable, float rectHeight, float rectScale, bool scaleToLength) : base(stiffness) { _scp = null; this.p1 = p1; this.p2 = p2; checkParticlesLocation(); _restLength = this.CurrLength; setCollidable(collidable, rectHeight, rectScale, scaleToLength); }
public SpringConstraintParticle(AbstractParticle p1, AbstractParticle p2, SpringConstraint p, float rectHeight, float rectScale, bool scaleToLength) : base(0, 0, 0, 0, 0, false, 1, 0.3f, 0) { this.p1 = p1; this.p2 = p2; lambda = new Vector(0, 0); avgVelocity = new Vector(0, 0); parent = p; this.RectScale = rectScale; this.RectHeight = rectHeight; this.scaleToLength = scaleToLength; this.FixedEndLimit = 0; rca = new Vector(0, 0); rcb = new Vector(0, 0); }
public static void resolveParticleParticle(AbstractParticle pa, AbstractParticle pb, Vector normal, float depth) { // a collision has occured. set the current positions to sample locations pa.curr = pa.samp; pb.curr = pb.samp; Vector mtd = normal * depth; float te = pa.Elasticity + pb.Elasticity; float sumInvMass = pa.InvMass + pb.InvMass; // the total friction in a collision is combined but clamped to [0,1] float tf = MathUtil.Clamp(1 - (pa.Friction + pb.Friction), 0, 1); // get the collision components, vn and vt Collision ca = pa.getComponents(normal); Collision cb = pb.getComponents(normal); // calculate the coefficient of restitution based on the mass, as the normal component Vector cbvn = cb.vn * ((te + 1) * pa.InvMass); Vector cavn = ca.vn * (pb.InvMass - te * pa.InvMass); Vector vnA = (cbvn + cavn) / sumInvMass; cavn = ca.vn * ((te + 1) * pb.InvMass); cbvn = cb.vn * (pa.InvMass - te * pb.InvMass); Vector vnB = (cavn + cbvn) / sumInvMass; // apply friction to the tangental component ca.vt *= tf; cb.vt *= tf; // scale the mtd by the ratio of the masses. heavier particles move less Vector mtdA = mtd * (pa.InvMass / sumInvMass); Vector mtdB = mtd * (-pb.InvMass / sumInvMass); // add the tangental component to the normal component for the new velocity vnA += ca.vt; vnB += cb.vt; if (!pa.Fixed) pa.resolveCollision(mtdA, vnA, normal, depth, -1, pb); if (!pb.Fixed) pb.resolveCollision(mtdB, vnB, normal, depth, 1, pa); }
private float getContactPointParam(AbstractParticle p) { float t = 0; if (p.ParticleType == 2) { t = closestParamPoint(p.curr); } else if (p.ParticleType == 1) { // go through the sides of the colliding rectangle as line segments int shortestIndex = 0; float[] paramList = new float[4]; float shortestDistance = float.PositiveInfinity; for (int i = 0; i < 4; i++) { setCorners((RectangleParticle) p, i); // check for closest points on SCP to side of rectangle float d = closestPtSegmentSegment(); if (d < shortestDistance) { shortestDistance = d; shortestIndex = i; paramList[i] = s; } } t = paramList[shortestIndex]; } return t; }
public override void resolveCollision(Vector mtd, Vector vel, Vector n, float d, int o, AbstractParticle p) { float t = getContactPointParam(p); float c1 = (1 - t); float c2 = t; // if one is fixed then move the other particle the entire way out of collision. // also, dispose of collisions at the sides of the scp. The higher the fixedEndLimit // value, the more of the scp not be effected by collision. if (p1.Fixed) { if (c2 <= this.FixedEndLimit) return; lambda = new Vector(mtd.X / c2, mtd.Y / c2); p2.curr += lambda; p2.Velocity = vel; } else if (p2.Fixed) { if (c1 <= this.FixedEndLimit) return; lambda = new Vector(mtd.X / c1, mtd.Y / c1); p1.curr += lambda; p1.Velocity = vel; // else both non fixed - move proportionally out of collision } else { float denom = (c1 * c1 + c2 * c2); if (denom == 0) return; lambda = new Vector(mtd.X / denom, mtd.Y / denom); p1.curr += lambda * c1; p2.curr += lambda * c2; // if collision is in the middle of SCP set the velocity of both end particles if (t == 0.5) { p1.Velocity = vel; p2.Velocity = vel; // otherwise change the velocity of the particle closest to contact } else { AbstractParticle corrParticle = (t < 0.5) ? p1 : p2; corrParticle.Velocity = vel; } } }
public SpringConstraint(AbstractParticle p1, AbstractParticle p2) : this(p1, p2, 0.5f) { }
public SpringConstraint(AbstractParticle p1, AbstractParticle p2, float stiffness) : this(p1, p2, stiffness, false, 1, 1, false) { }
public bool isConnectedTo(AbstractParticle p) { return (p == p1 || p == p2); }
public override void resolveCollision(Vector mtd, Vector vel, Vector n, float d, int o, AbstractParticle p) { // review the o (order) need here - its a hack fix base.resolveCollision(mtd, vel, n, d, o, p); resolve(n * Math.Sign(d * o)); }
public virtual void resolveCollision(Vector mtd, Vector vel, Vector n, float d, int o, AbstractParticle p) { curr += mtd; this.Velocity = vel; }
private static void sampVsNorm(AbstractParticle objA, AbstractParticle objB) { float s = 1 / (objA.MultiSample + 1); float t = s; objB.samp = objB.curr; for (int i = 0; i <= objA.MultiSample; i++) { objA.samp = new Vector(objA.prev.X + t * (objA.curr.X - objA.prev.X), objA.prev.Y + t * (objA.curr.Y - objA.prev.Y)); if (testTypes(objA, objB)) return; t += s; } // for (var i:int = 0; i <= objA.multisample; i++) }
private static void normVsNorm(AbstractParticle objA, AbstractParticle objB) { objA.samp = objA.curr; objB.samp = objB.curr; testTypes(objA, objB); }
private static bool testTypes(AbstractParticle objA, AbstractParticle objB) { if (objA.ParticleType == 1 && objB.ParticleType == 1) return testOBBvsOBB((RectangleParticle) objA, (RectangleParticle) objB); else if (objA.ParticleType == 2 && objB.ParticleType == 2) return testCirclevsCircle((CircleParticle) objA, (CircleParticle) objB); else if (objA.ParticleType == 1 && objB.ParticleType == 2) return testOBBvsCircle((RectangleParticle) objA, (CircleParticle) objB); else if (objA.ParticleType == 2 && objB.ParticleType == 1) return testOBBvsCircle((RectangleParticle) objB, (CircleParticle) objA); return false; }
public Body( AbstractParticle left, AbstractParticle right, int height, float lineWeight, int lineColor) { float cpx = (right.PX + left.PX) / 2; float cpy = right.PY; rgt = new CircleParticle(right.PX, right.PY, 1); rgt.SetStyle(3, lineColor, lineColor); lft = new CircleParticle(left.PX, left.PY, 1); lft.SetStyle(3, lineColor, lineColor); ctr = new CircleParticle(cpx, cpy, 1); ctr.Visible = false; top = new CircleParticle(cpx, cpy - height / 2, 1); top.Visible = false; bot = new CircleParticle(cpx, cpy + height / 2, 1); bot.Visible = false; // outer constraints SpringConstraint tr = new SpringConstraint(top, rgt, 1); tr.Visible = false; SpringConstraint rb = new SpringConstraint(rgt, bot, 1); rb.Visible = false; SpringConstraint bl = new SpringConstraint(bot, lft, 1); bl.Visible = false; SpringConstraint lt = new SpringConstraint(lft, top, 1); lt.Visible = false; // inner constrainst SpringConstraint ct = new SpringConstraint(top, this.Center, 1); ct.Visible = false; SpringConstraint cr = new SpringConstraint(rgt, this.Center, 1); cr.SetLine(lineWeight, lineColor); SpringConstraint cb = new SpringConstraint(bot, this.Center, 1); cb.Visible = false; SpringConstraint cl = new SpringConstraint(lft, this.Center, 1); cl.SetLine(lineWeight, lineColor); ctr.Collidable = false; top.Collidable = false; rgt.Collidable = false; bot.Collidable = false; lft.Collidable = false; this.Particles.Add(ctr); this.Particles.Add(top); this.Particles.Add(rgt); this.Particles.Add(bot); this.Particles.Add(lft); this.Constraints.Add(tr); this.Constraints.Add(rb); this.Constraints.Add(bl); this.Constraints.Add(lt); this.Constraints.Add(ct); this.Constraints.Add(cr); this.Constraints.Add(cb); this.Constraints.Add(cl); }