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;
}