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);
}
