corrade-vassal – Rev 1
?pathlinks?
/*
* Copyright (c) 2006-2014, openmetaverse.org
* All rights reserved.
*
* - Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* - Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
* - Neither the name of the openmetaverse.org nor the names
* of its contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
using System;
namespace OpenMetaverse
{
public class CoordinateFrame
{
public static readonly Vector3 X_AXIS = new Vector3(1f, 0f, 0f);
public static readonly Vector3 Y_AXIS = new Vector3(0f, 1f, 0f);
public static readonly Vector3 Z_AXIS = new Vector3(0f, 0f, 1f);
/// <summary>Origin position of this coordinate frame</summary>
public Vector3 Origin
{
get { return origin; }
set
{
if (!value.IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame.Origin assignment");
origin = value;
}
}
/// <summary>X axis of this coordinate frame, or Forward/At in grid terms</summary>
public Vector3 XAxis
{
get { return xAxis; }
set
{
if (!value.IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame.XAxis assignment");
xAxis = value;
}
}
/// <summary>Y axis of this coordinate frame, or Left in grid terms</summary>
public Vector3 YAxis
{
get { return yAxis; }
set
{
if (!value.IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame.YAxis assignment");
yAxis = value;
}
}
/// <summary>Z axis of this coordinate frame, or Up in grid terms</summary>
public Vector3 ZAxis
{
get { return zAxis; }
set
{
if (!value.IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame.ZAxis assignment");
zAxis = value;
}
}
protected Vector3 origin;
protected Vector3 xAxis;
protected Vector3 yAxis;
protected Vector3 zAxis;
#region Constructors
public CoordinateFrame(Vector3 origin)
{
this.origin = origin;
xAxis = X_AXIS;
yAxis = Y_AXIS;
zAxis = Z_AXIS;
if (!this.origin.IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame constructor");
}
public CoordinateFrame(Vector3 origin, Vector3 direction)
{
this.origin = origin;
LookDirection(direction);
if (!IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame constructor");
}
public CoordinateFrame(Vector3 origin, Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
{
this.origin = origin;
this.xAxis = xAxis;
this.yAxis = yAxis;
this.zAxis = zAxis;
if (!IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame constructor");
}
public CoordinateFrame(Vector3 origin, Matrix4 rotation)
{
this.origin = origin;
xAxis = rotation.AtAxis;
yAxis = rotation.LeftAxis;
zAxis = rotation.UpAxis;
if (!IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame constructor");
}
public CoordinateFrame(Vector3 origin, Quaternion rotation)
{
Matrix4 m = Matrix4.CreateFromQuaternion(rotation);
this.origin = origin;
xAxis = m.AtAxis;
yAxis = m.LeftAxis;
zAxis = m.UpAxis;
if (!IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame constructor");
}
#endregion Constructors
#region Public Methods
public void ResetAxes()
{
xAxis = X_AXIS;
yAxis = Y_AXIS;
zAxis = Z_AXIS;
}
public void Rotate(float angle, Vector3 rotationAxis)
{
Quaternion q = Quaternion.CreateFromAxisAngle(rotationAxis, angle);
Rotate(q);
}
public void Rotate(Quaternion q)
{
Matrix4 m = Matrix4.CreateFromQuaternion(q);
Rotate(m);
}
public void Rotate(Matrix4 m)
{
xAxis = Vector3.Transform(xAxis, m);
yAxis = Vector3.Transform(yAxis, m);
Orthonormalize();
if (!IsFinite())
throw new Exception("Non-finite in CoordinateFrame.Rotate()");
}
public void Roll(float angle)
{
Quaternion q = Quaternion.CreateFromAxisAngle(xAxis, angle);
Matrix4 m = Matrix4.CreateFromQuaternion(q);
Rotate(m);
if (!yAxis.IsFinite() || !zAxis.IsFinite())
throw new Exception("Non-finite in CoordinateFrame.Roll()");
}
public void Pitch(float angle)
{
Quaternion q = Quaternion.CreateFromAxisAngle(yAxis, angle);
Matrix4 m = Matrix4.CreateFromQuaternion(q);
Rotate(m);
if (!xAxis.IsFinite() || !zAxis.IsFinite())
throw new Exception("Non-finite in CoordinateFrame.Pitch()");
}
public void Yaw(float angle)
{
Quaternion q = Quaternion.CreateFromAxisAngle(zAxis, angle);
Matrix4 m = Matrix4.CreateFromQuaternion(q);
Rotate(m);
if (!xAxis.IsFinite() || !yAxis.IsFinite())
throw new Exception("Non-finite in CoordinateFrame.Yaw()");
}
public void LookDirection(Vector3 at)
{
LookDirection(at, Z_AXIS);
}
/// <summary>
///
/// </summary>
/// <param name="at">Looking direction, must be a normalized vector</param>
/// <param name="upDirection">Up direction, must be a normalized vector</param>
public void LookDirection(Vector3 at, Vector3 upDirection)
{
// The two parameters cannot be parallel
Vector3 left = Vector3.Cross(upDirection, at);
if (left == Vector3.Zero)
{
// Prevent left from being zero
at.X += 0.01f;
at.Normalize();
left = Vector3.Cross(upDirection, at);
}
left.Normalize();
xAxis = at;
yAxis = left;
zAxis = Vector3.Cross(at, left);
}
/// <summary>
/// Align the coordinate frame X and Y axis with a given rotation
/// around the Z axis in radians
/// </summary>
/// <param name="heading">Absolute rotation around the Z axis in
/// radians</param>
public void LookDirection(double heading)
{
yAxis.X = (float)Math.Cos(heading);
yAxis.Y = (float)Math.Sin(heading);
xAxis.X = (float)-Math.Sin(heading);
xAxis.Y = (float)Math.Cos(heading);
}
public void LookAt(Vector3 origin, Vector3 target)
{
LookAt(origin, target, new Vector3(0f, 0f, 1f));
}
public void LookAt(Vector3 origin, Vector3 target, Vector3 upDirection)
{
this.origin = origin;
Vector3 at = new Vector3(target - origin);
at.Normalize();
LookDirection(at, upDirection);
}
#endregion Public Methods
protected bool IsFinite()
{
if (xAxis.IsFinite() && yAxis.IsFinite() && zAxis.IsFinite())
return true;
else
return false;
}
protected void Orthonormalize()
{
// Make sure the axis are orthagonal and normalized
xAxis.Normalize();
yAxis -= xAxis * (xAxis * yAxis);
yAxis.Normalize();
zAxis = Vector3.Cross(xAxis, yAxis);
}
}
}