public static float CalcTargetFromPosition(ref Vector3D target, ref Vector3D position, ref Vector3D? front) { Vector3D f = new Vector3D(1, 0, 0); if (front != null) f = (Vector3D)front; Vector3D tar = target - position; tar.Z = 0; if (tar.LengthSQ > 0) tar.Normalize(); float dotAngle; dotAngle = f.DotProduct(tar); //Vector3D.Dot(ref front, ref tar, out dotAngle); Vector3D cross; cross = f.CrossProduct(tar); //Vector3D.Cross(ref front, ref tar, out cross); float angle = (float)Math.Acos(dotAngle); if (cross.Z < 0) angle = MathHelper.TwoPI - angle; return angle; }
/// <summary> Builds a right-handed look-at matrix.</summary> public void BuildCameraLookAtMatrixRH(Vector3D position, Vector3D target, Vector3D upVector) { Vector3D zaxis = position - target; zaxis.Normalize(); Vector3D xaxis = upVector.CrossProduct(zaxis); xaxis.Normalize(); Vector3D yaxis = zaxis.CrossProduct(xaxis); SetMInsecure(0,0,xaxis.X); SetMInsecure(1,0,yaxis.X); SetMInsecure(2,0,zaxis.X); SetMInsecure(3,0,0); SetMInsecure(0,1,xaxis.Y); SetMInsecure(1,1,yaxis.Y); SetMInsecure(2,1,zaxis.Y); SetMInsecure(3,1,0); SetMInsecure(0,2,xaxis.Z); SetMInsecure(1,2,yaxis.Z); SetMInsecure(2,2,zaxis.Z); SetMInsecure(3,2,0); SetMInsecure(0,3,-xaxis.DotProduct(position)); SetMInsecure(1,3,-yaxis.DotProduct(position)); SetMInsecure(2,3,-zaxis.DotProduct(position)); SetMInsecure(3,3,1.0f); }
private void UserAvatarMoveMouse(Position2D _position) { if (Reference.Viewer.GuiManager.Focused) { Reference.Viewer.ProtocolManager.AvatarConnection.Forward = false; return; } // Get target position from mouse cursor position. Vector3D targetPosition = new Vector3D(); Triangle3D triangle = new Triangle3D(); bool find = FindPositionFromMousePosition(_position, out targetPosition, out triangle); if (find == false) { return; } targetPosition.Y = 0; // Get user avatar position. Vector3D userPosition = m_userPosition; //userObject.Node.Position; userPosition.Y = 0; bool isRunLenght = false; // Create target vector. Vector3D targetVec = targetPosition - userPosition; if (targetVec.LengthSQ < (ignoreMoveArea * ignoreMoveArea)) { return; } else { if (targetVec.LengthSQ > (runLength * runLength)) isRunLenght = true; targetVec.Normalize(); } Vector3D baseVector = new Vector3D(0, 0, -1); Vector3D verticalVector = baseVector.CrossProduct(targetVec); bool flipVector = verticalVector.Y > 0; if (flipVector == false) { baseVector = new Vector3D(0, 0, 1); } radHeading = baseVector.DotProduct(targetVec) * div2pi; radHeadingSmoothReset = true; if (flipVector) { radHeading += pi; } UserAvatarRotation(0); Reference.Viewer.ProtocolManager.AvatarConnection.Forward = true; Reference.Viewer.ProtocolManager.AvatarConnection.Run = isRunLenght; }
//! multiplication operator by a vector public static Vector3D operator *(Vector3D lhs, Quaternion rhs) { Vector3D uv, uuv; Vector3D qvec = new Vector3D(rhs.X, rhs.Y, rhs.Z); uv = qvec.CrossProduct(lhs); uuv = qvec.CrossProduct(uv); uv *= (2.0f * rhs.W); uuv *= 2.0f; return lhs + uv + uuv; }