libremetaverse/libsecondlife/CoordinateFrame.cs

/*
 * Copyright (c) 2007, Second Life Reverse Engineering Team
 * 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 Second Life Reverse Engineering Team 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 libsecondlife
{
    public class CoordinateFrame
    {
        public LLVector3 Origin { get { return origin; } }
        public LLVector3 XAxis { get { return xAxis; } }
        public LLVector3 YAxis { get { return yAxis; } }
        public LLVector3 ZAxis { get { return zAxis; } }

        public LLVector3 AtAxis { get { return xAxis; } }
        public LLVector3 LeftAxis { get { return yAxis; } }
        public LLVector3 UpAxis { get { return zAxis; } }

        protected LLVector3 origin;
        protected LLVector3 xAxis;
        protected LLVector3 yAxis;
        protected LLVector3 zAxis;

        #region Constructors

        public CoordinateFrame()
        {
        }

        public CoordinateFrame(LLVector3 origin, LLVector3 xAxis, LLVector3 yAxis, LLVector3 zAxis)
        {
            this.origin = origin;
            this.xAxis = xAxis;
            this.yAxis = yAxis;
            this.zAxis = zAxis;
        }

        #endregion Constructors

        #region Public Methods

        public void ResetAxes()
        {
            xAxis = new LLVector3(1f, 0f, 0f);
            yAxis = new LLVector3(0f, 1f, 0f);
            zAxis = new LLVector3(0f, 0f, 1f);
        }

        public bool IsFinite()
        {
            if (xAxis.IsFinite() && yAxis.IsFinite() && zAxis.IsFinite())
                return true;
            else
                return false;
        }

        /// <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(LLVector3 at, LLVector3 upDirection)
        {
            // The two parameters cannot be parallel
            LLVector3 left = LLVector3.Cross(upDirection, at);
            if (left == LLVector3.Zero)
            {
                // Prevent left from being zero
                at.X += 0.01f;
                at = LLVector3.Norm(at);
                left = LLVector3.Cross(upDirection, at);
            }
            left = LLVector3.Norm(left);

            xAxis = at;
            yAxis = left;
            zAxis = LLVector3.Cross(at, left);
        }

        public void Orthonormalize()
        {
            // Make sure the axis are orthagonal and normalized
            xAxis = LLVector3.Norm(xAxis);
            yAxis -= xAxis * (xAxis * yAxis);
            yAxis = LLVector3.Norm(yAxis);
            zAxis = LLVector3.Cross(xAxis, yAxis);
        }

        public void Rotate(float angle, LLVector3 rotationAxis)
        {
            LLQuaternion q = new LLQuaternion(angle, rotationAxis);
            Rotate(q);
        }

        public void Rotate(LLQuaternion q)
        {
            LLMatrix3 m = new LLMatrix3(q);
            Rotate(m);
        }

        public void Rotate(LLMatrix3 m)
        {
            xAxis = LLVector3.Rot(xAxis, m);
            yAxis = LLVector3.Rot(yAxis, m);

            Orthonormalize();

            if (!IsFinite())
                throw new Exception("Non-finite in CoordinateFrame.Rotate()");
        }

        public void Roll(float angle)
        {
            LLQuaternion q = new LLQuaternion(angle, xAxis);
            LLMatrix3 m = new LLMatrix3(q);
            Rotate(m);

            if (!yAxis.IsFinite() || !zAxis.IsFinite())
                throw new Exception("Non-finite in CoordinateFrame.Roll()");
        }

        public void Pitch(float angle)
        {
            LLQuaternion q = new LLQuaternion(angle, yAxis);
            LLMatrix3 m = new LLMatrix3(q);
            Rotate(m);

            if (!xAxis.IsFinite() || !zAxis.IsFinite())
                throw new Exception("Non-finite in CoordinateFrame.Pitch()");
        }

        public void Yaw(float angle)
        {
            LLQuaternion q = new LLQuaternion(angle, zAxis);
            LLMatrix3 m = new LLMatrix3(q);
            Rotate(m);

            if (!xAxis.IsFinite() || !yAxis.IsFinite())
                throw new Exception("Non-finite in CoordinateFrame.Yaw()");
        }

        public void LookDirection(LLVector3 at)
        {
            LookDirection(at, new LLVector3(0f, 0f, 1f));
        }

        public void LookAt(LLVector3 origin, LLVector3 target, LLVector3 upDirection)
        {
            this.origin = origin;
            LLVector3 at = new LLVector3(target - origin);
            at = LLVector3.Norm(at);

            LookDirection(at, upDirection);
        }

        public void LookAt(LLVector3 origin, LLVector3 target)
        {
            LookAt(origin, target, new LLVector3(0f, 0f, 1f));
        }

        #endregion Public Methods
    }
}