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