void Physics::step( real_t dt )
{
std::vector< SphereBody* >::iterator it;
std::vector< SphereBody* >::iterator it2;
std::vector< PlaneBody* >::iterator pt;
std::vector< TriangleBody* >::iterator tt;
for (it = spheres.begin(); it != spheres.end(); ++it)
{
SphereBody *sb = *it;
for (tt = triangles.begin(); tt != triangles.end(); ++tt)
{
if (collides(*sb, **tt, collision_damping))
{
Vector3 n =
normalize(
cross(
(*tt)->vertices[0] - (*tt)->vertices[1],
(*tt)->vertices[1] - (*tt)->vertices[2]
)
);
Vector3 u = sb->velocity - 2 * dot(sb->velocity, n) * n;
sb->velocity = u - (collision_damping * u);
if (squared_length(sb->velocity) <= 1.0f)
sb->velocity = Vector3::Zero();
}
}
for (it2 = spheres.begin(); it2 != spheres.end(); ++it2)
{
if (collides(**it, **it2, collision_damping) && *it != *it2)
{
SphereBody *sb2 = *it2;
Vector3 v1 = sb->velocity - sb2->velocity;
Vector3 d = ((sb2->position - sb->position)
/ distance(sb->position, sb2->position));
Vector3 v22 = 2 * d
* (sb->mass / (sb->mass + sb2->mass))
* (dot(v1, d));
Vector3 u2 = sb2->velocity + v22;
Vector3 u1 =
((sb->mass * sb->velocity)
+ (sb2->mass * sb2->velocity)
- (sb2->mass * u2))
/ sb->mass;
sb->velocity = u1 - collision_damping * u1;
sb2->velocity = u2 - collision_damping * u2;
if (squared_length(sb->velocity) <= 1.0f)
sb->velocity = Vector3::Zero();
if (squared_length(sb2->velocity) <= 1.0f)
sb2->velocity = Vector3::Zero();
}
}
for (pt = planes.begin(); pt != planes.end(); ++pt)
{
if (collides(*sb, **pt, collision_damping))
{
Vector3 u = sb->velocity - 2 * dot(sb->velocity, (*pt)->normal) * (*pt)->normal;
sb->velocity = u - collision_damping * u;
if (squared_length(sb->velocity) <= 1.0f)
sb->velocity = Vector3::Zero();
}
}
sb->force = Vector3::Zero();
for (SpringList::iterator si = springs.begin(); si != springs.end(); ++si) {
(*si)->step(dt);
}
sb->apply_force(gravity, Vector3::Zero());
// si pas de force et velocite, ne pas faire les tests
State initMove = {sb->position, sb->velocity, sb->force / sb->mass};
State s = rk4(dt, initMove);
sb->position += s.x;
sb->sphere->position = sb->position;
sb->velocity += s.v;
real_t i = (2.0f / 5.0f) * sb->mass * (sb->radius * sb->radius);
Vector3 angular_accel = sb->torque / i;
State initRot = {Vector3::Zero(), sb->angular_velocity, angular_accel};
s = rk4(dt, initRot);
if (s.x != Vector3::Zero())
{
Quaternion q(s.x, length(s.x));
Quaternion qq = q * sb->orientation;
sb->orientation = qq;
sb->sphere->orientation = sb->orientation;
}
}
}
void Physics::rk4_update(SphereBody &s, real_t dt){
// Store Original Status
// -- position
Vector3 originalPosition = s.position;
Vector3 originalVelocity = s.velocity;
// -- rotation
Quaternion originalOrientation = s.orientation;
Vector3 originalAngVelocity = s.angular_velocity;
real_t I = 0.4 * s.mass * s.radius * s.radius;
//Step 1
// -- apply force
s.clear_force();
s.apply_force(gravity, Vector3::Zero());
for(size_t j=0; j<num_springs(); j++){
springs[j]->step(dt);
}
// -- position
Vector3 v_v1 = Vector3::Zero();
s.velocity = originalVelocity + v_v1;
Vector3 v_k1 = s.step_position(dt, 0.0);
s.position = originalPosition + v_k1 / 2.0;
// -- rotation
Vector3 a_v1 = Vector3::Zero();
s.angular_velocity = originalAngVelocity + a_v1;
Vector3 a_k1 = s.step_orientation(dt, 0.0);
s.orientation = rk4_orientation_update(originalOrientation, a_k1/2.0);
//Step 2
// -- apply force
s.clear_force();
s.apply_force(gravity, Vector3::Zero());
for(size_t j=0; j<num_springs(); j++){
springs[j]->step(dt);
}
// -- position
Vector3 v_v2 = (s.force / s.mass) * (dt / 2.0);
s.velocity = originalVelocity + v_v2;
Vector3 v_k2 = s.step_position(dt, 0.0);
s.position = originalPosition + v_k2 / 2.0;
// -- rotation
Vector3 a_v2 = (s.torque / I) * (dt / 2.0);
s.angular_velocity = originalAngVelocity + a_v2;
Vector3 a_k2 = s.step_orientation(dt, 0.0);
s.orientation = rk4_orientation_update(originalOrientation, a_k2/2.0);
//Step 3
// -- apply force
s.clear_force();
s.apply_force(gravity, Vector3::Zero());
for(size_t j=0; j<num_springs(); j++){
springs[j]->step(dt);
}
// -- position
Vector3 v_v3 = (s.force / s.mass) * (dt / 2.0);
s.velocity = originalVelocity + v_v3;
Vector3 v_k3 = s.step_position(dt, 0.0);
s.position = originalPosition + v_k3 / 2.0;
// -- rotation
Vector3 a_v3 = (s.torque / I) * (dt / 2.0);
s.angular_velocity = originalAngVelocity + a_v3;
Vector3 a_k3 = s.step_orientation(dt, 0.0);
s.orientation = rk4_orientation_update(originalOrientation, a_k3/1.0);
//Step 4
// -- apply force
s.clear_force();
s.apply_force(gravity, Vector3::Zero());
for(size_t j=0; j<num_springs(); j++){
springs[j]->step(dt);
}
// -- position
Vector3 v_v4 = (s.force / s.mass) * dt;
s.velocity = originalVelocity + v_v4;
Vector3 v_k4 = s.step_position(dt, 0.0);
s.position = originalPosition + v_k4 / 2.0;
// -- rotation
Vector3 a_v4 = (s.torque / I) * dt;
s.angular_velocity = originalAngVelocity + a_v4;
Vector3 a_k4 = s.step_orientation(dt, 0.0);
// Final Update
// -- position
Vector3 deltaPosition = v_k1 * (1.0/6.0) + v_k2 * (2.0/6.0) + v_k3 * (2.0/6.0) + a_k4 * (1.0/6.0);
Vector3 deltaVelocity = v_v1 * (1.0/6.0) + v_v2 * (2.0/6.0) + v_v3 * (2.0/6.0) + v_v4 * (1.0/6.0);
s.sphere->position = originalPosition + deltaPosition;
s.velocity = originalVelocity + deltaVelocity;
// -- rotation
Vector3 deltaRotation = a_k1 * (1.0/6.0) + a_k2 * (2.0/6.0) + a_k3 * (2.0/6.0) + a_k4 * (1.0/6.0);
Vector3 deltaAngularV = a_v1 * (1.0/6.0) + a_v2 * (2.0/6.0) + a_v3 * (2.0/6.0) + a_v4 * (1.0/6.0);
s.sphere->orientation = rk4_orientation_update(originalOrientation, deltaRotation/1.0);
s.angular_velocity = originalAngVelocity + deltaAngularV;
}