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