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