void Physics::set_forces(real_t dt)
{
//reset force and apply gravity
for (size_t i = 0; i < num_spheres(); i++) {
spheres[i]->force = Vector3::Zero();
spheres[i]->torque = Vector3::Zero();
spheres[i]->apply_force(gravity *
spheres[i]->mass, Vector3::Zero());
}
//apply spring forces
for (size_t i = 0; i < num_springs(); i++) {
springs[i]->step(dt);
}
}
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;
}
