void Physics::step( real_t dt )
{
// step the world forward by dt. Need to detect collisions, apply
// forces, and integrate positions and orientations.
for(size_t i=0; i<num_spheres(); i++){
//Update Spheres boundbox
Vector3 upper = spheres[i]->position + Vector3(spheres[i]->radius, spheres[i]->radius, spheres[i]->radius);
Vector3 lower = spheres[i]->position - Vector3(spheres[i]->radius, spheres[i]->radius, spheres[i]->radius);
spheres[i]->bound = BoundBox(lower, upper);
for(size_t j=0; j<num_spheres(); j++){
if(i!=j){
collides(*spheres[i], *spheres[j], collision_damping);
}
}
for(size_t j=0; j<num_planes(); j++){
collides(*spheres[i], *planes[j], collision_damping);
}
for(size_t j=0; j<num_triangles(); j++){
collides(*spheres[i], *triangles[j], collision_damping);
}
for(size_t j=0; j<num_models(); j++){
collides(*spheres[i], *models[j], collision_damping);
}
//RK4 Position & Orientation
rk4_update(*spheres[i], dt);
spheres[i]->clear_force();
}
}
void Physics::detect_collisions()
{
for (size_t i = 0; i < num_spheres(); i++) {
for (size_t j = 0; j < num_spheres(); j++) {
if (i != j) {
collides(*(spheres[i]), *(spheres[j]), collision_damping);
}
}
for (size_t j = 0; j < num_planes(); j++) {
collides(*(spheres[i]), *(planes[j]), collision_damping);
}
for (size_t j = 0; j < num_triangles(); j++) {
collides(*(spheres[i]), *(triangles[j]), collision_damping);
}
}
}
void Physics::save_initial_states()
{
for (size_t i = 0; i < num_spheres(); i++) {
spheres[i]->initial_velocity = spheres[i]->velocity;
spheres[i]->initial_position = spheres[i]->position;
spheres[i]->initial_angular_velocity = spheres[i]->angular_velocity;
spheres[i]->initial_orientation = spheres[i]->orientation;
}
}
void Physics::RK4(real_t dt) {
//reset states and derivatives
for (size_t i = 0; i < num_spheres(); i++) {
spheres[i]->state.dx = Vector3::Zero();
spheres[i]->state.dv = Vector3::Zero();
spheres[i]->state.dax = Vector3::Zero();
spheres[i]->state.dav = Vector3::Zero();
}
RK4_step(dt, 0.5, 1.0/6.0);
RK4_step(dt, 0.5, 1.0/3.0);
RK4_step(dt, 1.0, 1.0/3.0);
RK4_step(dt, 0.0, 1.0/6.0); //dummy value
for (size_t i = 0; i < num_spheres(); i++) {
spheres[i]->position =
spheres[i]->initial_position + (spheres[i]->state.dx);
spheres[i]->velocity =
spheres[i]->initial_velocity + (spheres[i]->state.dv);
Vector3 dax = spheres[i]->state.dax;
real_t x_radians = dax.x; //rotation around x axis
real_t y_radians = dax.y; //rotation around y axis
real_t z_radians = dax.z; //rotation around z axis
Quaternion qx = Quaternion(Vector3::UnitX(), x_radians);
Quaternion qy = Quaternion(Vector3::UnitY(), y_radians);
Quaternion qz = Quaternion(Vector3::UnitZ(), z_radians);
spheres[i]->orientation =
normalize(spheres[i]->initial_orientation * qz); //roll
spheres[i]->orientation =
normalize(spheres[i]->orientation * qx); //pitch
spheres[i]->orientation =
normalize(spheres[i]->orientation * qy); //yaw
spheres[i]->angular_velocity =
spheres[i]->initial_angular_velocity +
(spheres[i]->state.dav);
}
}
void Physics::step( real_t dt )
{
// Check collision between every sphere and other objects.
for (size_t i = 0; i < num_spheres(); i++) {
for (size_t j = 0; j < num_planes(); j++)
collides(*spheres[i], *planes[j], collision_damping);
for (size_t j = 0; j < num_triangles(); j++)
collides(*spheres[i], *triangles[j], collision_damping);
for (size_t j = i + 1; j < num_spheres(); j++) {
collides(*spheres[i], *spheres[j], collision_damping);
}
}
std::vector<State> states(num_spheres());
for (size_t i = 0; i < num_spheres(); i++) {
states[i].position = spheres[i]->position;
states[i].velocity = spheres[i]->velocity;
states[i].angular_velocity = spheres[i]->angular_velocity;
// Step dt using rk4 integral.
rk4_integrate(states[i], spheres[i], dt);
}
// Update every sphere's state after rk4 integral.
for (size_t i = 0; i < num_spheres(); i++) {
spheres[i]->position = states[i].position;
spheres[i]->velocity = states[i].velocity;
spheres[i]->angular_velocity = states[i].angular_velocity;
spheres[i]->update_orientation(states[i].angular_position);
spheres[i]->sphere->orientation = spheres[i]->orientation;
spheres[i]->sphere->position = states[i].position;
// Update the offset vector of this sphere in every spring it is attached to.
for (size_t j = 0; j < spheres[i]->spring_ptrs.size(); j++) {
spheres[i]->spring_ptrs[j]->update_offset(spheres[i], states[i].angular_position);
}
}
}
void Physics::RK4_step(real_t dt, real_t fraction, real_t weight) {
set_forces(dt); //param actually unnecessary?
for (size_t i = 0; i < num_spheres(); i++) {
spheres[i]->state.dx += weight
* spheres[i]->step_position(dt, fraction, collision_damping);
spheres[i]->state.dv += weight * dt
* spheres[i]->get_acceleration();
spheres[i]->state.dax += weight
* spheres[i]->step_orientation(dt, fraction, collision_damping);
spheres[i]->state.dav += weight * dt
* spheres[i]->get_angular_acceleration();
}
}
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::update_graphics()
{
for (size_t i = 0; i < num_spheres(); i++) {
spheres[i]->update_graphics();
}
}
