diff --git a/src/GakumasLocalify/Hook.cpp b/src/GakumasLocalify/Hook.cpp index 5d9f0c2..7d2e294 100644 --- a/src/GakumasLocalify/Hook.cpp +++ b/src/GakumasLocalify/Hook.cpp @@ -9,6 +9,9 @@ #include #include #include +#include +#include +#include #include #include #include "camera/camera.hpp" @@ -119,14 +122,30 @@ namespace GakumasLocal::HookMain { UnityResolve::UnityType::Camera* mainCameraCache = nullptr; UnityResolve::UnityType::Transform* cameraTransformCache = nullptr; - void CheckAndUpdateMainCamera() { + + void CheckAndUpdateMainCamera(UnityResolve::UnityType::Camera* fallbackCamera = nullptr) { if (!Config::enableFreeCamera) return; if (IsNativeObjectAlive(mainCameraCache) && IsNativeObjectAlive(cameraTransformCache)) return; - mainCameraCache = UnityResolve::UnityType::Camera::GetMain(); - cameraTransformCache = mainCameraCache->GetTransform(); + // 优先使用游戏传入的真实相机(渲染回调 / FOV hook 捕获)。 + // 注意:不要用 UnityResolve 的 managed Invoke 直接调 Camera.get_main / get_transform, + // Unity 6 下该调用缺少 MethodInfo* 参数,可能返回垃圾指针(崩溃根因)。 + if (!mainCameraCache || !IsNativeObjectAlive(mainCameraCache)) { + mainCameraCache = fallbackCamera; + } + // 不再回退到 Camera.GetMain()/GetCurrent():UnityResolve 的 managed Invoke 可能返回垃圾指针。 + // 相机只从游戏自己的调用捕获(EndCameraRendering 参数 / FOV hook / get_main hook)。 + // cameraTransformCache 由 Component.get_transform hook 在游戏调用时捕获。 + + if (!mainCameraCache) { + cameraTransformCache = nullptr; + } } + bool TryLookRotationQuat(const UnityResolve::UnityType::Vector3& forwardIn, + const UnityResolve::UnityType::Vector3& upIn, + UnityResolve::UnityType::Quaternion* outQuat); + Il2cppUtils::Resolution_t GetResolution() { static auto GetResolution = Il2cppUtils::GetMethod("UnityEngine.CoreModule.dll", "UnityEngine", "Screen", "get_currentResolution"); @@ -147,6 +166,12 @@ namespace GakumasLocal::HookMain { DEFINE_HOOK(void, Unity_set_fieldOfView, (UnityResolve::UnityType::Camera* self, float value)) { if (Config::enableFreeCamera) { + // self 是游戏传的真实相机,直接捕获。 + // Cinemachine 每帧会把镜头参数应用到输出相机,这里拿到的就是实际游戏相机。 + if (self != mainCameraCache) { + mainCameraCache = self; + cameraTransformCache = nullptr; // 相机变了,Transform 需要重新解析 + } if (self == mainCameraCache) { value = GKCamera::baseCamera.fov; } @@ -156,6 +181,10 @@ namespace GakumasLocal::HookMain { DEFINE_HOOK(float, Unity_get_fieldOfView, (UnityResolve::UnityType::Camera* self)) { if (Config::enableFreeCamera) { + if (self != mainCameraCache) { + mainCameraCache = self; + cameraTransformCache = nullptr; + } if (self == mainCameraCache) { static auto get_orthographic = reinterpret_cast(Il2cppUtils::il2cpp_resolve_icall( "UnityEngine.Camera::get_orthographic()" @@ -165,7 +194,9 @@ namespace GakumasLocal::HookMain { )); for (const auto& i : UnityResolve::UnityType::Camera::GetAllCamera()) { - // Log::DebugFmt("get_orthographic: %d", get_orthographic(i)); + if (get_orthographic) { + // Log::DebugFmt("get_orthographic: %d", get_orthographic(i)); + } // set_orthographic(i, false); Unity_set_fieldOfView_Orig(i, GKCamera::baseCamera.fov); } @@ -178,21 +209,159 @@ namespace GakumasLocal::HookMain { return Unity_get_fieldOfView_Orig(self); } + // 从游戏自己的调用里捕获真实对象指针: + // Camera.get_main / Component.get_transform 是带 MethodInfo* 尾参的 managed 方法, + // 用 hook(而不是 UnityResolve 的 managed Invoke)才能拿到可靠结果。 + DEFINE_HOOK(UnityResolve::UnityType::Camera*, Camera_get_main, (void* mtd)) { + auto ret = Camera_get_main_Orig(mtd); + if (Config::enableFreeCamera && ret && ret != mainCameraCache) { + mainCameraCache = ret; + cameraTransformCache = nullptr; + } + return ret; + } + + DEFINE_HOOK(UnityResolve::UnityType::Transform*, Component_get_transform, (void* self, void* mtd)) { + auto ret = Component_get_transform_Orig(self, mtd); + if (Config::enableFreeCamera && mainCameraCache && self == mainCameraCache) { + cameraTransformCache = ret; + } + return ret; + } + UnityResolve::UnityType::Transform* cacheTrans = nullptr; UnityResolve::UnityType::Quaternion cacheRotation{}; UnityResolve::UnityType::Vector3 cachePosition{}; UnityResolve::UnityType::Vector3 cacheForward{}; UnityResolve::UnityType::Vector3 cacheLookAt{}; + // 计算当前模式下的相机位置/朝向。返回 false 表示暂不可用(NaN/目标丢失)。 + bool ComputeFreeCameraPose(UnityResolve::UnityType::Vector3* pos, UnityResolve::UnityType::Quaternion* rot) { + using Vector3 = UnityResolve::UnityType::Vector3; + using Quaternion = UnityResolve::UnityType::Quaternion; + + Vector3 lookAt{}; + switch (GKCamera::GetCameraMode()) { + case GKCamera::CameraMode::FREE: { + *pos = GKCamera::baseCamera.GetPos(); + lookAt = GKCamera::baseCamera.GetLookAt(); + } break; + case GKCamera::CameraMode::FIRST_PERSON: { + if (!cacheTrans || !IsNativeObjectAlive(cacheTrans)) return false; + *pos = GKCamera::CalcFirstPersonPosition(cachePosition, cacheForward, GKCamera::firstPersonPosOffset); + lookAt = cacheLookAt; + } break; + case GKCamera::CameraMode::FOLLOW: { + lookAt = GKCamera::CalcFollowModeLookAt(cachePosition, GKCamera::followPosOffset); + *pos = GKCamera::CalcPositionFromLookAt(lookAt, GKCamera::followPosOffset); + } break; + default: + return false; + } + + if (!std::isfinite(pos->x) || !std::isfinite(pos->y) || !std::isfinite(pos->z) || + !std::isfinite(lookAt.x) || !std::isfinite(lookAt.y) || !std::isfinite(lookAt.z)) { + return false; + } + + const auto forward = Vector3(lookAt.x - pos->x, lookAt.y - pos->y, lookAt.z - pos->z); + return TryLookRotationQuat(forward, Vector3(0, 1, 0), rot); + } + + bool IsCameraControlTransform(void* self) { + if (!self || !cameraTransformCache) return false; + return self == cameraTransformCache; + } + + // Cinemachine 把最终相机状态写入 Unity 相机的唯一入口(CinemachineBrain.PushStateToUnityCamera)。 + // CameraState 是大结构体,按值传递时经隐藏指针传入(hook 的 state 参数即指向该副本), + // 直接改副本,游戏会用自己内部的原生路径应用——不需要调用任何 Transform API,绝对安全。 + // 字段偏移来自 hook-reference dump.cs: + // 0x00 Lens.FieldOfView | 0x48 RawPosition | 0x54 RawOrientation + // 0x64 PositionDampingBypass | 0x74 PositionCorrection | 0x80 OrientationCorrection + DEFINE_HOOK(void, CinemachineBrain_PushStateToUnityCamera, (void* self, void* state, void* mtd)) { + if (Config::enableFreeCamera && state) { + CheckAndUpdateMainCamera(); + + using Vector3 = UnityResolve::UnityType::Vector3; + using Quaternion = UnityResolve::UnityType::Quaternion; + auto* rawPos = reinterpret_cast(static_cast(state) + 0x48); + auto* rawRot = reinterpret_cast(static_cast(state) + 0x54); + auto* damping = reinterpret_cast(static_cast(state) + 0x64); + auto* posCorr = reinterpret_cast(static_cast(state) + 0x74); + auto* rotCorr = reinterpret_cast(static_cast(state) + 0x80); + auto* fov = reinterpret_cast(static_cast(state) + 0x00); + + if (ComputeFreeCameraPose(rawPos, rawRot)) { + *damping = Vector3(0, 0, 0); + *posCorr = Vector3(0, 0, 0); + *rotCorr = Quaternion(0, 0, 0, 1); + *fov = GKCamera::baseCamera.fov; + } + } + + return CinemachineBrain_PushStateToUnityCamera_Orig(self, state, mtd); + } + + // Cinemachine 3(Unity 6)用 SetPositionAndRotation 一次性写相机位置+旋转, + // set_position/set_rotation 拦截不到相机,必须在这里接管。 + DEFINE_HOOK(void, Unity_SetPositionAndRotation_Injected, (UnityResolve::UnityType::Transform* self, + UnityResolve::UnityType::Vector3* position, + UnityResolve::UnityType::Quaternion* rotation)) { + if (Config::enableFreeCamera) { + CheckAndUpdateMainCamera(); + + const auto isControl = IsCameraControlTransform(self); + if (isControl) { + UnityResolve::UnityType::Vector3 pos{}; + UnityResolve::UnityType::Quaternion rot{}; + if (ComputeFreeCameraPose(&pos, &rot)) { + *position = pos; + *rotation = rot; + } + } + } + + return Unity_SetPositionAndRotation_Injected_Orig(self, position, rotation); + } + + // 兜底:游戏若用局部坐标写相机(rig 挂载场景),位置同样接管。 + DEFINE_HOOK(void, Unity_set_localPosition_Injected, (UnityResolve::UnityType::Transform* self, + UnityResolve::UnityType::Vector3* data)) { + if (Config::enableFreeCamera) { + CheckAndUpdateMainCamera(); + + const auto isControl = IsCameraControlTransform(self); + if (isControl) { + const auto cameraMode = GKCamera::GetCameraMode(); + if (cameraMode == GKCamera::CameraMode::FIRST_PERSON) { + if (cacheTrans && IsNativeObjectAlive(cacheTrans)) { + *data = GKCamera::CalcFirstPersonPosition(cachePosition, cacheForward, GKCamera::firstPersonPosOffset); + } + } + else if (cameraMode == GKCamera::CameraMode::FOLLOW) { + auto newLookAtPos = GKCamera::CalcFollowModeLookAt(cachePosition, GKCamera::followPosOffset); + auto pos = GKCamera::CalcPositionFromLookAt(newLookAtPos, GKCamera::followPosOffset); + data->x = pos.x; + data->y = pos.y; + data->z = pos.z; + } + else { + auto& origCameraPos = GKCamera::baseCamera.pos; + data->x = origCameraPos.x; + data->y = origCameraPos.y; + data->z = origCameraPos.z; + } + } + } + + return Unity_set_localPosition_Injected_Orig(self, data); + } + DEFINE_HOOK(void, Unity_set_rotation_Injected, (UnityResolve::UnityType::Transform* self, UnityResolve::UnityType::Quaternion* value)) { if (Config::enableFreeCamera) { - static auto lookat_injected = reinterpret_cast( - Il2cppUtils::il2cpp_resolve_icall( - "UnityEngine.Transform::Internal_LookAt_Injected(UnityEngine.Vector3&,UnityEngine.Vector3&)")); - static auto worldUp = UnityResolve::UnityType::Vector3(0, 1, 0); - - if (cameraTransformCache == self) { + const auto isControl = IsCameraControlTransform(self); + if (isControl) { const auto cameraMode = GKCamera::GetCameraMode(); if (cameraMode == GKCamera::CameraMode::FIRST_PERSON) { if (cacheTrans && IsNativeObjectAlive(cacheTrans)) { @@ -203,22 +372,39 @@ namespace GakumasLocal::HookMain { static GakumasLocal::Misc::FixedSizeQueue recordsY(60); const auto newY = GKCamera::CheckNewY(cacheLookAt, true, recordsY); UnityResolve::UnityType::Vector3 newCacheLookAt{cacheLookAt.x, newY, cacheLookAt.z}; - lookat_injected(self, &newCacheLookAt, &worldUp); - return; + const auto pos = GKCamera::CalcFirstPersonPosition( + cachePosition, cacheForward, GKCamera::firstPersonPosOffset); + UnityResolve::UnityType::Quaternion q{}; + const auto forward = UnityResolve::UnityType::Vector3( + newCacheLookAt.x - pos.x, newCacheLookAt.y - pos.y, newCacheLookAt.z - pos.z); + if (TryLookRotationQuat(forward, UnityResolve::UnityType::Vector3(0, 1, 0), &q)) { + *value = q; + } } } } else if (cameraMode == GKCamera::CameraMode::FOLLOW) { auto newLookAtPos = GKCamera::CalcFollowModeLookAt(cachePosition, GKCamera::followPosOffset, true); - lookat_injected(self, &newLookAtPos, &worldUp); - return; + const auto pos = GKCamera::CalcPositionFromLookAt(newLookAtPos, GKCamera::followPosOffset); + UnityResolve::UnityType::Quaternion q{}; + const auto forward = UnityResolve::UnityType::Vector3( + newLookAtPos.x - pos.x, newLookAtPos.y - pos.y, newLookAtPos.z - pos.z); + if (TryLookRotationQuat(forward, UnityResolve::UnityType::Vector3(0, 1, 0), &q)) { + *value = q; + } } else { auto& origCameraLookat = GKCamera::baseCamera.lookAt; - lookat_injected(self, &origCameraLookat, &worldUp); - // Log::DebugFmt("fov: %f, target: %f", Unity_get_fieldOfView_Orig(mainCameraCache), GKCamera::baseCamera.fov); - return; + auto& origCameraPos = GKCamera::baseCamera.pos; + UnityResolve::UnityType::Quaternion q{}; + const auto forward = UnityResolve::UnityType::Vector3( + origCameraLookat.x - origCameraPos.x, + origCameraLookat.y - origCameraPos.y, + origCameraLookat.z - origCameraPos.z); + if (TryLookRotationQuat(forward, UnityResolve::UnityType::Vector3(0, 1, 0), &q)) { + *value = q; + } } } } @@ -229,7 +415,8 @@ namespace GakumasLocal::HookMain { if (Config::enableFreeCamera) { CheckAndUpdateMainCamera(); - if (cameraTransformCache == self) { + const auto isControl = IsCameraControlTransform(self); + if (isControl) { const auto cameraMode = GKCamera::GetCameraMode(); if (cameraMode == GKCamera::CameraMode::FIRST_PERSON) { if (cacheTrans && IsNativeObjectAlive(cacheTrans)) { @@ -257,6 +444,64 @@ namespace GakumasLocal::HookMain { return Unity_set_position_Injected_Orig(self, data); } + // 用 forward/up 构造 Unity 兼容的四元数(等价 Quaternion.LookRotation)。 + // 返回 false 表示 forward 与 up 平行(垂直看天/看地),此时不应写入旋转,保持当前朝向。 + bool TryLookRotationQuat(const UnityResolve::UnityType::Vector3& forwardIn, + const UnityResolve::UnityType::Vector3& upIn, + UnityResolve::UnityType::Quaternion* outQuat) { + using Vector3 = UnityResolve::UnityType::Vector3; + using Quaternion = UnityResolve::UnityType::Quaternion; + + Vector3 forward = forwardIn; + const auto fwdLenSq = forward.x * forward.x + forward.y * forward.y + forward.z * forward.z; + if (fwdLenSq < 1e-8f) { + return false; + } + const auto invFwdLen = 1.0f / std::sqrt(fwdLenSq); + forward = Vector3(forward.x * invFwdLen, forward.y * invFwdLen, forward.z * invFwdLen); + + Vector3 right = Vector3( + upIn.y * forward.z - upIn.z * forward.y, + upIn.z * forward.x - upIn.x * forward.z, + upIn.x * forward.y - upIn.y * forward.x); + const auto rightLenSq = right.x * right.x + right.y * right.y + right.z * right.z; + if (rightLenSq < 1e-8f) { + return false; + } + const auto invRightLen = 1.0f / std::sqrt(rightLenSq); + right = Vector3(right.x * invRightLen, right.y * invRightLen, right.z * invRightLen); + + const Vector3 up = Vector3( + forward.y * right.z - forward.z * right.y, + forward.z * right.x - forward.x * right.z, + forward.x * right.y - forward.y * right.x); + + // 旋转矩阵列向量(right/up/forward)-> 标准矩阵转四元数 + const float m00 = right.x, m01 = up.x, m02 = forward.x; + const float m10 = right.y, m11 = up.y, m12 = forward.y; + const float m20 = right.z, m21 = up.z, m22 = forward.z; + + const float tr = m00 + m11 + m22; + if (tr > 0.0f) { + const float s = std::sqrt(tr + 1.0f) * 2.0f; + *outQuat = Quaternion((m21 - m12) / s, (m02 - m20) / s, (m10 - m01) / s, 0.25f * s); + return true; + } + if (m00 > m11 && m00 > m22) { + const float s = std::sqrt(1.0f + m00 - m11 - m22) * 2.0f; + *outQuat = Quaternion(0.25f * s, (m01 + m10) / s, (m02 + m20) / s, (m21 - m12) / s); + return true; + } + if (m11 > m22) { + const float s = std::sqrt(1.0f + m11 - m00 - m22) * 2.0f; + *outQuat = Quaternion((m01 + m10) / s, 0.25f * s, (m12 + m21) / s, (m02 - m20) / s); + return true; + } + const float s = std::sqrt(1.0f + m22 - m00 - m11) * 2.0f; + *outQuat = Quaternion((m02 + m20) / s, (m12 + m21) / s, 0.25f * s, (m10 - m01) / s); + return true; + } + #ifdef GKMS_WINDOWS DEFINE_HOOK(void*, InternalSetOrientationAsync, (void* retstr, void* self, int type, void* c, void* tc, void* mtd)) { switch (Config::gameOrientation) { @@ -280,10 +525,16 @@ namespace GakumasLocal::HookMain { DEFINE_HOOK(void, EndCameraRendering, (void* ctx, void* camera, void* method)) { EndCameraRendering_Orig(ctx, camera, method); - if (Config::enableFreeCamera && mainCameraCache) { - Unity_set_fieldOfView_Orig(mainCameraCache, GKCamera::baseCamera.fov); - if (GKCamera::GetCameraMode() == GKCamera::CameraMode::FIRST_PERSON) { - mainCameraCache->SetNearClipPlane(0.001f); + if (Config::enableFreeCamera) { + // Camera.main 可能拿不到 3.0.0 实际渲染相机,用当前正在渲染的相机兜底 + CheckAndUpdateMainCamera(static_cast(camera)); + if (mainCameraCache && IsNativeObjectAlive(mainCameraCache)) { + // 注意:不能在渲染回调里写相机 Transform(UnityPlayer 会崩), + // 位置/朝向由独立驱动线程负责,这里只保留 FOV/近裁剪等属性设置。 + Unity_set_fieldOfView_Orig(mainCameraCache, GKCamera::baseCamera.fov); + if (GKCamera::GetCameraMode() == GKCamera::CameraMode::FIRST_PERSON) { + mainCameraCache->SetNearClipPlane(0.001f); + } } } } @@ -1555,16 +1806,23 @@ namespace GakumasLocal::HookMain { return PictureBookLiveSelectScreenPresenter_MoveLiveScene_Orig(self, produceLive, isPlayCharacterFocusCamera, mtd); } + // 进入 Live 时歌词默认显示:仅对每个 Presenter 实例的第一次设置强制 true, + // 之后(玩家的手动开关)原样透传,不再覆盖关闭操作。 + std::unordered_set g_lyricsInitialForced{}; + DEFINE_HOOK(void, LiveSceneModel_set_IsLyricsActive, (void* self, bool value, void* mtd)) { - return LiveSceneModel_set_IsLyricsActive_Orig(self, Config::dbgMode && Config::unlockAllLive ? true : value, mtd); + return LiveSceneModel_set_IsLyricsActive_Orig(self, value, mtd); } DEFINE_HOOK(void, LiveScenePresenter_SetLyricsActive, (void* self, bool value, void* mtd)) { - return LiveScenePresenter_SetLyricsActive_Orig(self, Config::dbgMode && Config::unlockAllLive ? true : value, mtd); + if (Config::dbgMode && Config::unlockAllLive && self && g_lyricsInitialForced.insert(self).second) { + value = true; + } + return LiveScenePresenter_SetLyricsActive_Orig(self, value, mtd); } DEFINE_HOOK(void, LiveSceneContentView_SetLyricsTextActive, (void* self, bool value, void* mtd)) { - return LiveSceneContentView_SetLyricsTextActive_Orig(self, Config::dbgMode && Config::unlockAllLive ? true : value, mtd); + return LiveSceneContentView_SetLyricsTextActive_Orig(self, value, mtd); } // std::string lastMusicId; @@ -2449,6 +2707,17 @@ namespace GakumasLocal::HookMain { "UnityEngine.Transform::set_position_Injected(UnityEngine.Vector3&)")); ADD_HOOK(Unity_set_rotation_Injected, Il2cppUtils::il2cpp_resolve_icall( "UnityEngine.Transform::set_rotation_Injected(UnityEngine.Quaternion&)")); + ADD_HOOK(Unity_SetPositionAndRotation_Injected, Il2cppUtils::il2cpp_resolve_icall( + "UnityEngine.Transform::SetPositionAndRotation_Injected(UnityEngine.Vector3&,UnityEngine.Quaternion&)")); + ADD_HOOK(Unity_set_localPosition_Injected, Il2cppUtils::il2cpp_resolve_icall( + "UnityEngine.Transform::set_localPosition_Injected(UnityEngine.Vector3&)")); + ADD_HOOK(Camera_get_main, Il2cppUtils::GetMethodPointer("UnityEngine.CoreModule.dll", "UnityEngine", + "Camera", "get_main")); + ADD_HOOK(CinemachineBrain_PushStateToUnityCamera, + Il2cppUtils::GetMethodPointer("Cinemachine.dll", "Cinemachine", + "CinemachineBrain", "PushStateToUnityCamera")); + ADD_HOOK(Component_get_transform, Il2cppUtils::GetMethodPointer("UnityEngine.CoreModule.dll", "UnityEngine", + "Component", "get_transform")); ADD_HOOK(Unity_get_fieldOfView, Il2cppUtils::GetMethodPointer("UnityEngine.CoreModule.dll", "UnityEngine", "Camera", "get_fieldOfView")); ADD_HOOK(Unity_set_fieldOfView, Il2cppUtils::GetMethodPointer("UnityEngine.CoreModule.dll", "UnityEngine", diff --git a/src/GakumasLocalify/camera/camera.cpp b/src/GakumasLocalify/camera/camera.cpp index ebd8ba7..23e5d26 100644 --- a/src/GakumasLocalify/camera/camera.cpp +++ b/src/GakumasLocalify/camera/camera.cpp @@ -1,6 +1,7 @@ #include "baseCamera.hpp" #include "camera.hpp" #include +#include #include "../Misc.hpp" #include "../BaseDefine.h" #include "../../platformDefine.hpp" @@ -411,6 +412,14 @@ namespace GKCamera { const auto currentY = targetPos.y; static auto lastRetY = currentY; + // NaN/Inf 防护:目标 Y 非法时保持上一次有效值,避免污染平滑队列后相机飞走 + if (!std::isfinite(currentY)) { + return std::isfinite(lastRetY) ? lastRetY : 0.0f; + } + if (!std::isfinite(lastRetY)) { + lastRetY = currentY; + } + if (followModeY == FollowModeY::APPLY_Y) { lastRetY = currentY; return currentY; @@ -423,6 +432,10 @@ namespace GKCamera { recordsY.Push(currentY); } + if (!std::isfinite(currentAvg)) { + return lastRetY; + } + if (abs(currentY - currentAvg) < 0.02) { return lastRetY; } @@ -460,7 +473,17 @@ namespace GKCamera { // 计算角色的右方向 Vector3 up(0, 1, 0); // Y轴方向 - Vector3 right = forward.cross(up).Normalize(); + Vector3 right = forward.cross(up); + + // 修正归一化:原 UnityResolve::Vector3::Normalize 除的是平方长度,且零向量会产出 NaN + const auto rightLenSq = right.x * right.x + right.y * right.y + right.z * right.z; + if (rightLenSq < 1e-8f) { + // forward 与 up 平行/接近或为零向量,无法求右向,退化为仅高度偏移 + return Vector3(position.x, position.y + offset.y, position.z); + } + const auto invRightLen = 1.0f / std::sqrt(rightLenSq); + right = Vector3(right.x * invRightLen, right.y * invRightLen, right.z * invRightLen); + Vector3 fwd = forward; Vector3 pos = position;