public Rotator(int colA, int colB) : base(false) { this.CollideInternal = true; ctr = new Vector(555, 175); rectComposite = new RectComposite(ctr, colA, colB); addComposite(rectComposite); circA = new CircleParticle(ctr.X, ctr.Y, 5, false, 1, 0.3f, 0); circA.SetStyle(1, colA, colB); this.Particles.Add(circA); rectA = new RectangleParticle(555, 160, 10, 10, 0, false, 3, 0.3f, 0); rectA.SetStyle(1, colA, colB); this.Particles.Add(rectA); connectorA = new SpringConstraint(rectComposite.PA, rectA, 1, false, 1, 1, false); connectorA.SetStyle(2, colB); this.Constraints.Add(connectorA); rectB = new RectangleParticle(555, 190, 10, 10, 0, false, 3, 0.3f, 0); rectB.SetStyle(1, colA, colB); this.Particles.Add(rectB); connectorB = new SpringConstraint(rectComposite.PC, rectB, 1, false, 1, 1, false); connectorB.SetStyle(2, colB); this.Constraints.Add(connectorB); }
public AbstractParticle(float x, float y, bool isFixed, float mass, float elasticity, float friction) : base() { interval = new Interval(0, 0); curr = new Vector(x, y); prev = new Vector(x, y); samp = new Vector(0, 0); temp = new Vector(0, 0); this.Fixed = isFixed; forces = new Vector(0, 0); _collision_vn = new Vector(0, 0); _collision_vt = new Vector(0, 0); collision = new Collision(_collision_vn, _collision_vt); this.Collidable = true; this.Mass = mass; this.Elasticity = elasticity; this.Friction = friction; //setStyle(); _center = new Vector(0, 0); _multisample = 0; }
public WheelParticle(float x, float y, float radius, bool fixed_, float mass, float elasticity, float friction, float traction) : base(x, y, radius, fixed_, mass, elasticity, friction) { tan = new Vector(0, 0); normSlip = new Vector(0, 0); rp = new RimParticle(radius, 2); this.Traction = traction; }
public void rotateByRadian(float angleRadians, Vector center) { foreach (AbstractParticle p in this.Particles) { float radius = p.center().distance(center); float angle = getRelativeAngle(center, p.center()) + angleRadians; p.PX = ((float) Math.Cos(angle) * radius) + center.X; p.PY = ((float) Math.Sin(angle) * radius) + center.Y; } }
public RimParticle(float r, float mt) { curr = new Vector(r, 0); prev = new Vector(0, 0); sp = 0; av = 0; maxTorque = mt; wr = r; }
public RectangleParticle(float x, float y, float width, float height, float rotation, bool fixed_, float mass, float elasticity, float friction) : base(x, y, fixed_, mass, elasticity, friction) { _extents[0] = width / 2; _extents[1] = height / 2; axes[0] = new Vector(0, 0); axes[1] = new Vector(0, 0); this.Radian = rotation; }
private static Vector closestVertexOnOBB(Vector p, RectangleParticle r) { Vector d = p - r.samp; Vector q = new Vector(r.samp.X, r.samp.Y); for (int i = 0; i < 2; i++) { float dist = d.Dot(r.axes[i]); if (dist >= 0) dist = r.extents(i); else if (dist < 0) dist = -r.extents(i); q += (r.axes[i] * dist); } return q; }
public CarDemo() { int alpha = 255; int colA = Color.FromArgb(alpha, 51, 58, 51).ToArgb(); int colB = Color.FromArgb(alpha, 51, 102, 170).ToArgb(); int colC = Color.FromArgb(alpha, 170, 187, 187).ToArgb(); int colD = Color.FromArgb(alpha, 102, 153, 170).ToArgb(); int colE = Color.FromArgb(alpha, 119, 136, 119).ToArgb(); int colPad = Color.FromArgb(alpha, 153, 102, 51).ToArgb(); APEngine.init((float) 1 / 4); Vector massLessForces = new Vector(0, 3); APEngine.addMasslessForce(massLessForces); Surfaces surfaces = new Surfaces(colA, colB, colC, colD, colE, colPad); APEngine.addGroup(surfaces); Bridge bridge = new Bridge(colB, colC, colD); APEngine.addGroup(bridge); Capsule capsule = new Capsule(colC); APEngine.addGroup(capsule); rotator = new Rotator(colB, colE); APEngine.addGroup(rotator); SwingDoor swingDoor = new SwingDoor(colC); APEngine.addGroup(swingDoor); car = new Car(colC, colE); APEngine.addGroup(car); car.addCollidable(surfaces); car.addCollidable(bridge); car.addCollidable(rotator); car.addCollidable(swingDoor); car.addCollidable(capsule); capsule.addCollidable(surfaces); capsule.addCollidable(bridge); capsule.addCollidable(rotator); capsule.addCollidable(swingDoor); }
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); }
public RectComposite(Vector ctr, int colA, int colB) : base() { float rw = 75; float rh = 18; float rad = 4; // going clockwise from left top.. cpA = new CircleParticle(ctr.X - rw / 2, ctr.Y - rh / 2, rad, true, 1, 0.3f, 0); cpB = new CircleParticle(ctr.X + rw / 2, ctr.Y - rh / 2, rad, true, 1, 0.3f, 0); cpC = new CircleParticle(ctr.X + rw / 2, ctr.Y + rh / 2, rad, true, 1, 0.3f, 0); cpD = new CircleParticle(ctr.X - rw / 2, ctr.Y + rh / 2, rad, true, 1, 0.3f, 0); cpA.SetStyle(0, 0, colA); cpB.SetStyle(0, 0, colA); cpC.SetStyle(0, 0, colA); cpD.SetStyle(0, 0, colA); sprA = new SpringConstraint(cpA, cpB, 0.5f, true, rad * 2, 1, false); sprB = new SpringConstraint(cpB, cpC, 0.5f, true, rad * 2, 1, false); sprC = new SpringConstraint(cpC, cpD, 0.5f, true, rad * 2, 1, false); sprD = new SpringConstraint(cpD, cpA, 0.5f, true, rad * 2, 1, false); sprA.SetStyle(0, 0, colA); sprB.SetStyle(0, 0, colA); sprC.SetStyle(0, 0, colA); sprD.SetStyle(0, 0, colA); this.Particles.Add(cpA); this.Particles.Add(cpB); this.Particles.Add(cpC); this.Particles.Add(cpD); this.Constraints.Add(sprA); this.Constraints.Add(sprB); this.Constraints.Add(sprC); this.Constraints.Add(sprD); }
public override void Integrate(float dt2, Vector force, Vector masslessForce, float damping) { base.Integrate(dt2, force, masslessForce, damping); foreach (Composite c in _composites) c.Integrate(dt2, force, masslessForce, damping); }
public void addForce(Vector f) { Vector tmp = f * this.InvMass; forces += tmp; }
public Interval getProjection(Vector axis) { float radius = _extents[0] * Math.Abs(axis.Dot(axes[0])) + _extents[1] * Math.Abs(axis.Dot(axes[1])); float c = samp.Dot(axis); interval.min = c - radius; interval.max = c + radius; return interval; }
public virtual void resolveCollision(Vector mtd, Vector vel, Vector n, float d, int o, AbstractParticle p) { curr += mtd; this.Velocity = vel; }
private float closestParamPoint(Vector c) { Vector ab = p2.curr - p1.curr; float t = (ab.Dot(c - p1.curr)) / (ab.Dot(ab)); return MathUtil.Clamp(t, 0, 1); }
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 Composite() : base() { delta = new Vector(0, 0); }
public Vector center() { _center = new Vector(this.PX, this.PY); return _center; }
public Collision getComponents(Vector collisionNormal) { Vector vel = this.Velocity; float vdotn = collisionNormal.Dot(vel); collision.vn = collisionNormal * vdotn; collision.vt = vel - collision.vn; return collision; }
private void resolve(Vector n) { // this is the tangent vector at the rim particle tan = new Vector(-rp.curr.Y, rp.curr.X); // normalize so we can scale by the rotational speed tan = tan.normalize(); // velocity of the wheel's surface Vector wheelSurfaceVelocity = tan * rp.Speed; // the velocity of the wheel's surface relative to the ground this.Velocity += wheelSurfaceVelocity; Vector combinedVelocity = this.Velocity; // the wheel's comb velocity projected onto the contact normal float cp = combinedVelocity.Cross(n); // set the wheel's spinspeed to track the ground tan *= cp; rp.prev = rp.curr - tan; // some of the wheel's torque is removed and converted into linear displacement float slipSpeed = (1 - _traction) * rp.Speed; normSlip = new Vector(slipSpeed * n.Y, slipSpeed * n.X); curr += normSlip; rp.Speed = rp.Speed * _traction; }
public override void update(float dt, Vector force, Vector masslessForce, float damping) { base.update(dt, force, masslessForce, damping); rp.update(dt); }
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 Collision(Vector vn, Vector vt) { this.vn = vn; this.vt = vt; }
public virtual void Integrate(float dt2, Vector force, Vector masslessForce, float damping) { foreach (AbstractParticle p in _particles) p.update(dt2, force, masslessForce, damping); }
public void rotateByAngle(float angleDegrees, Vector center) { float angleRadians = MathUtil.ToRadians(angleDegrees); rotateByRadian(angleRadians, center); }
public void updatePosition() { Vector c = parent.Center; curr = new Vector(c.X, c.Y); this.Width = (scaleToLength) ? parent.CurrLength * this.RectScale : parent.RestLength * this.RectScale; this.Height = this.RectHeight; this.Radian = parent.Radian; }
public void addMasslessForce(Vector f) { forces += f; }
private float getRelativeAngle(Vector center, Vector p) { delta = new Vector(p.X - center.X, p.Y - center.Y); return (float) Math.Atan2(delta.Y, delta.X); }
public virtual void update(float dt2, Vector force, Vector masslessForce, float damping) { if (this.Fixed) return; // global forces addForce(force); addMasslessForce(masslessForce); // integrate temp = curr; Vector nv = this.Velocity + (forces *= dt2); curr += nv *= damping; prev = temp; // clear the forces forces = new Vector(0, 0); }