void CollisionResolver::resolveContact(CollisionContact& contact)
{
float linearInertiaLeft = 0, angularInertiaLeft = 0;
float linearInertiaRight = 0, angularInertiaRight = 0;
calculateInertia(dynamic_cast<PhysicsBody&>(contact.getLeft().getBody()), linearInertiaLeft, angularInertiaLeft);
if ( contact.hasRight() )
calculateInertia(dynamic_cast<PhysicsBody&>(contact.getRight().getBody()), linearInertiaRight, angularInertiaRight);
float totalInertia = linearInertiaLeft + angularInertiaLeft
+ linearInertiaRight + angularInertiaRight;
float invIntertia = 1 / totalInertia;
float linearMoveLeft = contact.mPenetration * linearInertiaLeft * invIntertia;
float linearMoveRight = -contact.mPenetration * linearInertiaRight * invIntertia;
{
Body& body = contact.getLeft().getBody();
body.setPosition(body.getPosition() + contact.mNormal * linearMoveLeft);
}
if ( contact.hasRight() )
{
Body& body = contact.getRight().getBody();
body.setPosition(body.getPosition() + contact.mNormal * linearMoveRight);
}
}
bool Heightfield::init(std::string heightmap, float mass, Vector3F scale)
{
// Load heightmap image
fipImage *image = new fipImage;
if (!image->load(heightmap.c_str()))
return false;
// Convert height data
float *heightvalues = new float[image->getWidth() * image->getHeight()];
for (int y = 0; y < image->getHeight(); y++)
{
for (int x = 0; x < image->getWidth(); x++)
{
RGBQUAD color;
image->getPixelColor(x, image->getHeight() - y - 1, &color);
heightvalues[x + y * image->getWidth()] = ((float)color.rgbRed + color.rgbGreen + color.rgbBlue) / 3.0f;
}
}
// Create heightfield
terrain = new btHeightfieldTerrainShape(image->getWidth(), image->getHeight(), heightvalues, 256, 1, true, false);
terrain->setLocalScaling(btVector3(scale.x, scale.y, scale.z));
shape = terrain;
this->mass = mass;
calculateInertia();
delete image;
return true;
}
bool Sphere::init(float radius, float mass)
{
shape = new btSphereShape(radius);
this->mass = mass;
calculateInertia();
return true;
}
