/// A solid capsule
typedef struct s2Capsule
{
s2Vec2 point1, point2;
float radius;
s2MakeCapsule(s2Vec2 p1, s2Vec2 p2, float radius)
{
s2Polygon shape = {0};
shape.vertices[0] = p1;
shape.vertices[1] = p2;
s2Vec2 axis = s2NormalizeChecked(s2Sub(p2, p1));
s2Vec2 normal = s2RightPerp(axis);
shape.normals[0] = normal;
shape.normals[1] = s2Neg(normal);
shape.count = 2;
shape.radius = radius;
return shape;
}
s2MassData s2ComputeCircleMass(const s2Circle* shape, float density)
{
float rr = shape->radius * shape->radius;
s2MassData massData;
massData.mass = density s2_pi rr;
massData.center = shape->point;
// inertia about the local origin
massData.I = massData.mass (0.5f rr + s2Dot(shape->point, shape->point));
return massData;
}
s2MassData s2ComputeCapsuleMass(const s2Capsule* shape, float density)
{
float radius = shape->radius;
float rr = radius * radius;
s2Vec2 p1 = shape->point1;
s2Vec2 p2 = shape->point2;
float length = s2Length(s2Sub(p2, p1));
float ll = length * length;
s2MassData massData;
massData.mass = density (s2_pi radius + 2.0f length) radius;
massData.center.x = 0.5f * (p1.x + p2.x);
massData.center.y = 0.5f * (p1.y + p2.y);
// two offset half circles, both halves add up to full circle and each half is offset by half length
// half circles = 2 (1/4 radius*radius + 1/4 lengthlength)
// rectangle = (width*width lengthlength)/12
float circleInertia = 0.5f * (rr + ll);
float boxInertia = (4.0f * rr + ll) / 12.0f;
massData.I = massData.mass * (circleInertia + boxInertia);
return massData;
}
Comments