const MassProperties& Cone::getMassProperties(ref<const Material> material) const
{
if (massPropertiesCached && (density == material->density()))
return massProperties;
density = material->density();
Real volume = consts::Pi*Math::sqr(_radius)*_height/3.0;
massProperties.mass = volume*density;
const Real m = massProperties.mass;
Matrix3 Ibody;
Ibody.e(1,1) = (3.0/20.0)*m*Math::sqr(_radius) + (3.0/80.0)*m*Math::sqr(_height);
Ibody.e(2,2) = Ibody.e(1,1);
Ibody.e(3,3) = (3.0/10.0)*m*Math::sqr(_radius);
massProperties.setIbody(Ibody);
massProperties.centerOfMass = Point3(0.0,0.0,0.0);
massPropertiesCached = true;
return massProperties;
}
Vector2 lh::transform(const Matrix3& matrix, Vector2& vector, float z)
{
return Vector2(
matrix.a() * vector.x + matrix.d() * vector.y + matrix.g() * z,
matrix.b() * vector.x + matrix.e() * vector.y + matrix.h() * z);
}
Vector2 rh::transform(const Matrix3& matrix, Vector2& vector, float z)
{
return Vector2(
vector.x * matrix.a() + vector.y * matrix.b() + z * matrix.c(),
vector.x * matrix.d() + vector.y * matrix.e() + z * matrix.f());
}