@Override
public Vector3f supportPointLocalNegative(Vector3f direction) {
if (direction.lengthSquared() != 0) direction.normalize();
Vector3f v = QuatMath.transform(collisionshape.getInverseRotation(), direction);
float vy = 0;
float capstart =
(float)
(halfheight
/ Math.sqrt(
radius * radius
+ halfheight
* halfheight)); // VecMath.normalize(new Vector2f(radius,
// halfheight)).y;
if (Math.abs(direction.y) > capstart) {
float len = (float) new Vector2f(direction.x / capstart, direction.z / capstart).length();
vy = (float) (Math.sqrt(radius * radius - len * len));
}
vy += halfheight;
return new Vector3f(-v.x * radius, (v.y < 0 ? vy : -vy), -v.z * radius);
}
@Override
public void resolve(CollisionManifold<Vector3f> manifold) {
RigidBody3 A = (RigidBody3) manifold.getObjects().getFirst();
RigidBody3 B = (RigidBody3) manifold.getObjects().getSecond();
Vector3f normal = manifold.getCollisionNormal();
// velAlongNormal = (B - A) dot normal
float velAlongNormal =
(B.getLinearVelocity().x - A.getLinearVelocity().x) * normal.x
+ (B.getLinearVelocity().y - A.getLinearVelocity().y) * normal.y
+ (B.getLinearVelocity().z - A.getLinearVelocity().z) * normal.z;
if (velAlongNormal > 0) return;
float e = Math.min(A.getRestitution(), B.getRestitution());
float j = (-(1 + e) * velAlongNormal) / (A.getInverseMass() + B.getInverseMass());
Vector3f impulse = VecMath.scale(normal, j);
B.applyCentralImpulse(impulse);
impulse.negate();
A.applyCentralImpulse(impulse);
}