예제 #1
0
파일: physics.cpp 프로젝트: Khady/p5
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;
        }
    }
}
예제 #2
0
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;
}