Fix implementation

Did not account for existing angle
Add fatalError to Position3n as Matrix is not yet available

Author: STREGA

Sources/GameMath/3D Types (New)/Position3n.swift

@@ -117,8 +117,7 @@ public extension Position3n where Scalar: FloatingPoint {
      */
     @inlinable
     func rotated(around anchor: Self = .zero, by rotation: Rotation3n<Scalar>) -> Self {
-        let d = self.distance(from: anchor)
-        return anchor.moved(d, toward: rotation.forward)
+        fatalError("Not implemented")
     }
 
     /** Rotates `self` around an anchor position.

Sources/GameMath/3D Types/Position3.swift

@@ -157,8 +157,14 @@ public extension Position3 {
      */
     @inlinable
     func rotated(around anchor: Self = .zero, by rotation: Quaternion) -> Self {
+        #if false
+        // TODO: This implementation is less accurate, needs testing to see if there's any performance benefits
         let d = self.distance(from: anchor)
-        return anchor.moved(d, toward: rotation.forward)
+        let currentRotation = Quaternion(direction: Direction3(from: anchor, to: self))
+        return anchor.moved(d, toward: (rotation * currentRotation).forward)
+        #else
+        return (Matrix4x4(position: anchor) * Matrix4x4(rotation: rotation)) * self
+        #endif
     }
 
     /** Rotates `self` around an anchor position.