public Vec3 GetVelocityAtWorldPoint(Vec3 p)
{
Vec3 directionToPoint = p - WorldCenter;
Vec3 relativeAngularVel = Vec3.Cross(AngularVelocity, directionToPoint);
return(LinearVelocity + relativeAngularVel);
}
public void ApplyForceAtWorldPoint(Vec3 force, Vec3 point)
{
Force += force * Mass;
Torque += Vec3.Cross(point - WorldCenter, force);
SetToAwake();
}
public void ApplyLinearImpulseAtWorldPoint(Vec3 impulse, Vec3 point)
{
LinearVelocity += impulse * InvMass;
AngularVelocity += InvInertiaWorld * Vec3.Cross(point - WorldCenter, impulse);
SetToAwake();
}
public void Render(Transform tx, bool awake, Render render)
{
Transform world = Transform.Mul(tx, local);
Vec3[] vertices = new Vec3[8] {
new Vec3(-e.x, -e.y, -e.z),
new Vec3(-e.x, -e.y, e.z),
new Vec3(-e.x, e.y, -e.z),
new Vec3(-e.x, e.y, e.z),
new Vec3(e.x, -e.y, -e.z),
new Vec3(e.x, -e.y, e.z),
new Vec3(e.x, e.y, -e.z),
new Vec3(e.x, e.y, e.z)
};
for (int i = 0; i < 36; i += 3)
{
Vec3 a = Transform.Mul(world, vertices[kBoxIndices[i]]);
Vec3 b = Transform.Mul(world, vertices[kBoxIndices[i + 1]]);
Vec3 c = Transform.Mul(world, vertices[kBoxIndices[i + 2]]);
Vec3 n = Vec3.Normalize(Vec3.Cross(b - a, c - a));
//render->SetPenColor( 0.2f, 0.4f, 0.7f, 0.5f );
//render->SetPenPosition( a.x, a.y, a.z );
//render->Line( b.x, b.y, b.z );
//render->Line( c.x, c.y, c.z );
//render->Line( a.x, a.y, a.z );
render.SetTriNormal(n.x, n.y, n.z);
render.Triangle(a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z);
}
}
//--------------------------------------------------------------------------------------------------
public static Mat3 Inverse(Mat3 m)
{
Vec3 tmp0, tmp1, tmp2;
double detinv;
tmp0 = Vec3.Cross(m.ey, m.ez);
tmp1 = Vec3.Cross(m.ez, m.ex);
tmp2 = Vec3.Cross(m.ex, m.ey);
detinv = 1 / Vec3.Dot(m.ez, tmp2);
return(new Mat3(
tmp0.x * detinv, tmp1.x * detinv, tmp2.x * detinv,
tmp0.y * detinv, tmp1.y * detinv, tmp2.y * detinv,
tmp0.z * detinv, tmp1.z * detinv, tmp2.z * detinv
));
}
// http://box2d.org/2014/02/computing-a-basis/
public static void ComputeBasis(Vec3 a, ref Vec3 b, ref Vec3 c)
{
// Suppose vector a has all equal components and is a unit vector: a = (s, s, s)
// Then 3*s*s = 1, s = sqrt(1/3) = 0.57735027. This means that at least one component of a
// unit vector must be greater or equal to 0.57735027. Can use SIMD select operation.
if (Math.Abs(a.x) >= (0.57735027))
{
b.Set(a.y, -a.x, 0);
}
else
{
b.Set(0, a.z, -a.y);
}
b = Vec3.Normalize(b);
c = Vec3.Cross(a, b);
}
public void Solve()
{
for (int i = 0; i < Contacts.Count; ++i)
{
ContactConstraintState cs = Contacts[i];
Vec3 vA = Velocities[cs.indexA].v;
Vec3 wA = Velocities[cs.indexA].w;
Vec3 vB = Velocities[cs.indexB].v;
Vec3 wB = Velocities[cs.indexB].w;
for (int j = 0; j < cs.contactCount; ++j)
{
ContactState c = cs.contacts[j];
// relative velocity at contact
Vec3 dv = vB + Vec3.Cross(wB, c.rb) - vA - Vec3.Cross(wA, c.ra);
// Friction
if (EnableFriction)
{
{
double lambda = -Vec3.Dot(dv, cs.tangentVectors) * c.tangentMass;
// Calculate frictional impulse
double maxLambda = cs.friction * c.normalImpulse;
// Clamp frictional impulse
double oldPT = c.tangentImpulse;
c.tangentImpulse = Clamp(-maxLambda, maxLambda, oldPT + lambda);
lambda = c.tangentImpulse - oldPT;
// Apply friction impulse
Vec3 impulse = cs.tangentVectors * lambda;
vA -= impulse * cs.mA;
wA -= cs.iA * Vec3.Cross(c.ra, impulse);
vB += impulse * cs.mB;
wB += cs.iB * Vec3.Cross(c.rb, impulse);
}
{
double lambda = -Vec3.Dot(dv, cs.bitangentVectors) * c.bitangentMass;
// Calculate frictional impulse
double maxLambda = cs.friction * c.normalImpulse;
// Clamp frictional impulse
double oldPT = c.bitangentImpulse;
c.bitangentImpulse = Clamp(-maxLambda, maxLambda, oldPT + lambda);
lambda = c.bitangentImpulse - oldPT;
// Apply friction impulse
Vec3 impulse = cs.bitangentVectors * lambda;
vA -= impulse * cs.mA;
wA -= cs.iA * Vec3.Cross(c.ra, impulse);
vB += impulse * cs.mB;
wB += cs.iB * Vec3.Cross(c.rb, impulse);
}
}
// Normal
{
dv = vB + Vec3.Cross(wB, c.rb) - vA - Vec3.Cross(wA, c.ra);
// Normal impulse
double vn = Vec3.Dot(dv, cs.normal);
// Factor in positional bias to calculate impulse scalar j
double lambda = c.normalMass * (-vn + c.bias);
// Clamp impulse
double tempPN = c.normalImpulse;
c.normalImpulse = Math.Max(tempPN + lambda, 0);
lambda = c.normalImpulse - tempPN;
// Apply impulse
Vec3 impulse = cs.normal * lambda;
vA -= impulse * cs.mA;
wA -= cs.iA * Vec3.Cross(c.ra, impulse);
vB += impulse * cs.mB;
wB += cs.iB * Vec3.Cross(c.rb, impulse);
}
}
Velocities[cs.indexA] = new VelocityState {
v = vA, w = wA
};
Velocities[cs.indexB] = new VelocityState {
v = vB, w = wB
};
}
}
public void PreSolve(double dt)
{
for (int i = 0; i < Contacts.Count; ++i)
{
ContactConstraintState cs = Contacts[i];
Vec3 vA = Velocities[cs.indexA].v;
Vec3 wA = Velocities[cs.indexA].w;
Vec3 vB = Velocities[cs.indexB].v;
Vec3 wB = Velocities[cs.indexB].w;
for (int j = 0; j < cs.contactCount; ++j)
{
ContactState c = cs.contacts[j];
// Precalculate JM^-1JT for contact and friction constraints
Vec3 raCn = Vec3.Cross(c.ra, cs.normal);
Vec3 rbCn = Vec3.Cross(c.rb, cs.normal);
double nm = cs.mA + cs.mB;
nm += Vec3.Dot(raCn, cs.iA * raCn) + Vec3.Dot(rbCn, cs.iB * rbCn);
c.normalMass = Invert(nm);
{
Vec3 raCt = Vec3.Cross(cs.tangentVectors, c.ra);
Vec3 rbCt = Vec3.Cross(cs.tangentVectors, c.rb);
var tm = nm + Vec3.Dot(raCt, cs.iA * raCt) + Vec3.Dot(rbCt, cs.iB * rbCt);
c.tangentMass = Invert(tm);
}
{
Vec3 raCt = Vec3.Cross(cs.bitangentVectors, c.ra);
Vec3 rbCt = Vec3.Cross(cs.bitangentVectors, c.rb);
var tm = nm + Vec3.Dot(raCt, cs.iA * raCt) + Vec3.Dot(rbCt, cs.iB * rbCt);
c.bitangentMass = Invert(tm);
}
// Precalculate bias factor
c.bias = -Q3_BAUMGARTE * (1 / dt) * Math.Min(0, c.penetration + Q3_PENETRATION_SLOP);
// Warm start contact
Vec3 P = cs.normal * c.normalImpulse;
if (EnableFriction)
{
P += cs.tangentVectors * c.tangentImpulse;
P += cs.bitangentVectors * c.bitangentImpulse;
}
vA -= P * cs.mA;
wA -= cs.iA * Vec3.Cross(c.ra, P);
vB += P * cs.mB;
wB += cs.iB * Vec3.Cross(c.rb, P);
// Add in restitution bias
double dv = Vec3.Dot(vB + Vec3.Cross(wB, c.rb) - vA - Vec3.Cross(wA, c.ra), cs.normal);
if (dv < -1)
{
c.bias += -(cs.restitution) * dv;
}
}
Velocities[cs.indexA] = new VelocityState {
v = vA, w = wA
};
Velocities[cs.indexB] = new VelocityState {
v = vB, w = wB
};
}
}
public void Set(Vec3 a, Vec3 b, Vec3 c)
{
normal = Vec3.Normalize(Vec3.Cross(b - a, c - a));
distance = Vec3.Dot(normal, a);
}