예제 #1
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;
}
예제 #2
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;
        }
    }
}
예제 #3
0
void TestSphereBody::testSphereBody() {
	Core::resetCore();
	SphereBody *sphere = new SphereBody("Sphere");
	QCOMPARE(sphere->getParameters().size(), 14);
	QVERIFY(sphere->getName().compare("Sphere") == 0);
	QVERIFY(sphere->getInputValues().size() == 0);
	QVERIFY(sphere->getOutputValues().size() == 0);
	QCOMPARE(sphere->getParameters().size(), 14);
	QVERIFY(sphere->getParameter("CenterOfMass") != 0);
	QVERIFY(sphere->getParameter("Position") != 0);
	QVERIFY(sphere->getParameter("Orientation") != 0);
	QVERIFY(sphere->getParameter("DynamicFriction") != 0);
	QVERIFY(sphere->getParameter("StaticFriction") != 0);
	QVERIFY(sphere->getParameter("Elasticity") != 0);
	QVERIFY(sphere->getParameter("Dynamic") != 0);
	QVERIFY(sphere->getParameter("Name") != 0);
	QVERIFY(sphere->getParameter("Mass") != 0);
	QVERIFY(sphere->getParameter("Color") != 0);
	DoubleValue *radius = dynamic_cast<DoubleValue*>(sphere->getParameter("Radius"));
	QVERIFY(radius != 0);

	QCOMPARE(radius->get(), 0.0);
	QCOMPARE(sphere->getCollisionObjects().size(), 1);
	QCOMPARE(sphere->getGeometries().size(), 1);

	SphereGeom *sphereGeom = dynamic_cast<SphereGeom*>(sphere->getGeometries().at(0));	
	QVERIFY(sphereGeom != 0);
	QCOMPARE(sphereGeom->getRadius(), 0.0);
	
	CollisionObject *collisionObject = sphere->getCollisionObjects().at(0);
	QVERIFY(collisionObject->getHostBody() == sphere);
	QVERIFY(collisionObject->getOwner() == 0);
	SphereGeom *collisionObjectGeom = dynamic_cast<SphereGeom*>(collisionObject->getGeometry());
	QVERIFY(collisionObjectGeom != 0);
	QCOMPARE(collisionObjectGeom->getRadius(), 0.0);

	radius->set(1.0);
	QCOMPARE(collisionObjectGeom->getRadius(), 0.0);
	QCOMPARE(sphereGeom->getRadius(), 0.0);

	sphere->setup();
	QVERIFY(collisionObject->getHostBody() == sphere);
	QVERIFY(collisionObject->getOwner() == sphere);
	QCOMPARE(collisionObjectGeom->getRadius(), 1.0);
	QCOMPARE(sphereGeom->getRadius(), 1.0);
	
	SphereBody *sphere2 = new SphereBody("Sphere2", 5.14);
	DoubleValue *radius2 = dynamic_cast<DoubleValue*>(sphere2->getParameter("Radius"));
	QVERIFY(radius2 != 0);
	QCOMPARE(radius2->get(), 5.14);
	SphereGeom *sphereGeom2 = dynamic_cast<SphereGeom*>(sphere2->getCollisionObjects().at(0)->getGeometry());

	QCOMPARE(sphereGeom2->getRadius(), 5.14);
	QCOMPARE(sphere2->getParameters().size(), 14);
	sphere2->getParameter("Position")->setValueFromString("(1.11, 2.2, 3.9)");
	Vector3DValue *position = dynamic_cast<Vector3DValue*>(sphere2->getParameter("Position"));
	QVERIFY(position != 0);
	QCOMPARE(position->getX(), 1.11);
	QCOMPARE(position->getY(), 2.2);
	QCOMPARE(position->getZ(), 3.9);	

	SphereBody *copy = dynamic_cast<SphereBody*>(sphere2->createCopy());	
	QVERIFY(copy != 0);
	QCOMPARE(copy->getParameters().size(), 14);
	QVERIFY(copy->getCollisionObjects().size() == 1);
	QVERIFY(copy->getGeometries().size() == 1);

	Vector3DValue *copyPosition = dynamic_cast<Vector3DValue*>(copy->getParameter("Position"));
	QVERIFY(copyPosition != 0);
	QCOMPARE(copyPosition->getX(), 1.11);
	QCOMPARE(copyPosition->getY(), 2.2);
	QCOMPARE(copyPosition->getZ(), 3.9);
	
	SphereGeom *copyGeometry = dynamic_cast<SphereGeom*>(copy->getCollisionObjects().at(0)->getGeometry());
	QVERIFY(copyGeometry != 0);
	QCOMPARE(copyGeometry->getRadius(), 5.14);

	CollisionObject *copyCollisionObject = copy->getCollisionObjects().at(0);
	QVERIFY(copyCollisionObject->getHostBody() == copy);
	QVERIFY(copyCollisionObject->getOwner() == 0);	

	copy->setup();
	QVERIFY(copyCollisionObject->getHostBody() == copy);
	QVERIFY(copyCollisionObject->getOwner() == copy);

	radius2->set(-10.0);
	sphere2->setup();
	QVERIFY(sphereGeom2->getRadius() > 0.0);

	delete copy;
	delete sphere2;
	delete sphere;
}