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