pointToWorldFrame method

Vec3 pointToWorldFrame(
  1. Vec3 localPoint,
  2. Vec3? result
)

Convert a local body point to world frame.

Implementation

Vec3 pointToWorldFrame(Vec3 localPoint, Vec3? result){
  result ??= Vec3();
  quaternion.vmult(localPoint, result);
  result.vadd(position, result);
  return result;
}