void Direct3D10Shader::setVar(const string& name, const Matrix44& value)
{
Matrix44 columnMajorValue = value;
columnMajorValue.transpose();
effect->GetVariableByName(name.data())->AsMatrix()->SetMatrix(columnMajorValue.getData());
}
Matrix44 Matrix44::getRotationOnly()
{
Matrix44 trans = *this;
trans.transpose();
Matrix44 inv = *this;
inv.inverse();
return trans * inv;
}
void State::integrate(State& state, float t, float dt)
{
Derivative a = Derivative::evaluate(state, t);
Derivative b = Derivative::evaluate(state, t, dt*0.5f, a);
Derivative c = Derivative::evaluate(state, t, dt*0.5f, b);
Derivative d = Derivative::evaluate(state, t, dt, c);
state.position += 1.0f/6.0f * dt * (a.velocity + 2.0f*(b.velocity + c.velocity) + d.velocity);
state.momentum += 1.0f/6.0f * dt * (a.force + 2.0f*(b.force + c.force) + d.force);
state.orientation += 1.0f/6.0f * dt * (a.spin + 2.0f*(b.spin + c.spin) + d.spin);
state.angularMomentum += 1.0f/6.0f * dt * (a.torque + 2.0f*(b.torque + c.torque) + d.torque);
const float e = 0.2;
const float u = 0.97;
std::vector<Collision> collision = collisions(state);
const float scale = 1.f/float(collision.size());
for(auto& c : collision)
{
if(c.direction.dot(state.momentum) > 0.f)
continue;
state.position += c.direction * c.magnitude;
const Vector3 r = c.point - state.position;
Vector3 velocityAtPoint = state.velocity + state.angularVelocity.cross(r);
const float vn = min (0.f, velocityAtPoint.dot(c.direction));
Matrix44 rotation = state.orientation.matrix();
Matrix44 transposeRotation = rotation.transpose();
Matrix44 i = rotation * state.inverseInertiaTensor * transposeRotation;
const float k = state.inverseMass + r.cross(c.direction).dot(i * r.cross(c.direction));
const float j = -(1+e) * vn / k;
state.momentum += c.direction * j * scale;
state.angularMomentum += r.cross(c.direction) * j * scale;
Vector3 tangentVelocity = velocityAtPoint - c.direction * velocityAtPoint.dot(c.direction);
if(tangentVelocity.lengthSquared() > epsilonSquared)
{
Vector3 tangent = tangentVelocity.unit();
const float vt = velocityAtPoint.dot(tangent);
const float kt = state.inverseMass + r.cross(tangent).dot(i * r.cross(tangent));
float jt = clamp(-vt/kt, -u*j, u*j);
state.momentum += tangent * jt * scale;
state.angularMomentum += r.cross(tangent) * jt * scale;
}
}
state.recalculate();
}