/// <summary>
/// Creates a quaternion from the provided euler angle (pitch/yaw/roll) rotation.
/// </summary>
/// <param name="xAngle">Pitch angle of rotation.</param>
/// <param name="yAngle">Yar angle of rotation.</param>
/// <param name="zAngle">Roll angle of rotation.</param>
/// <param name="order">The order in which rotations will be applied. Different rotations can be created depending
/// on the order.</param>
/// <returns>Quaternion that can rotate an object to the specified angles.</returns>
public static Quaternion FromEuler(Degree xAngle, Degree yAngle, Degree zAngle,
EulerAngleOrder order = EulerAngleOrder.YXZ)
{
EulerAngleOrderData l = EA_LOOKUP[(int)order];
Radian halfXAngle = xAngle * 0.5f;
Radian halfYAngle = yAngle * 0.5f;
Radian halfZAngle = zAngle * 0.5f;
float cx = MathEx.Cos(halfXAngle);
float sx = MathEx.Sin(halfXAngle);
float cy = MathEx.Cos(halfYAngle);
float sy = MathEx.Sin(halfYAngle);
float cz = MathEx.Cos(halfZAngle);
float sz = MathEx.Sin(halfZAngle);
Quaternion[] quats = new Quaternion[3];
quats[0] = new Quaternion(sx, 0.0f, 0.0f, cx);
quats[1] = new Quaternion(0.0f, sy, 0.0f, cy);
quats[2] = new Quaternion(0.0f, 0.0f, sz, cz);
return (quats[l.a] * quats[l.b]) * quats[l.c];
}
/// <summary>
/// Converts the quaternion rotation into axis/angle rotation.
/// </summary>
/// <param name="rotation">Quaternion to convert.</param>
/// <param name="axis">Axis around which the rotation is performed.</param>
/// <param name="angle">Amount of rotation.</param>
public static void ToAxisAngle(Quaternion rotation, out Vector3 axis, out Degree angle)
{
rotation.ToAxisAngle(out axis, out angle);
}
private static void DrawSphericalJoint(SphericalJoint joint)
{
Vector3 target = GetAnchor(joint, JointBody.Target);
Vector3 anchor = GetAnchor(joint, JointBody.Anchor);
Vector3 center = target;
Rigidbody rigidbody = joint.GetRigidbody(JointBody.Target);
if (rigidbody != null)
center = rigidbody.SceneObject.Position;
Gizmos.Color = Color.White;
Gizmos.DrawSphere(center, 0.05f);
Gizmos.Color = Color.Yellow;
Gizmos.DrawSphere(target, 0.05f);
Gizmos.DrawSphere(anchor, 0.05f);
Gizmos.Color = Color.Green;
Gizmos.DrawLine(target, center);
Gizmos.Color = Color.Green;
if (joint.EnableLimit)
{
LimitConeRange limit = joint.Limit;
Radian zAngle = MathEx.Min(new Degree(360), limit.ZLimitAngle * 2.0f);
Radian yAngle = MathEx.Min(new Degree(360), limit.YLimitAngle * 2.0f);
Gizmos.Transform = joint.SceneObject.WorldTransform;
Gizmos.DrawWireArc(Vector3.Zero, Vector3.ZAxis, 0.25f, zAngle * -0.5f + new Degree(90), zAngle);
Gizmos.DrawWireArc(Vector3.Zero, Vector3.YAxis, 0.25f, yAngle * -0.5f + new Degree(90), yAngle);
Gizmos.Color = Color.Red;
Radian remainingZAngle = new Degree(360) - zAngle;
Radian remainingYAngle = new Degree(360) - yAngle;
Gizmos.DrawWireArc(Vector3.Zero, Vector3.ZAxis, 0.25f, zAngle * 0.5f + new Degree(90), remainingZAngle);
Gizmos.DrawWireArc(Vector3.Zero, Vector3.YAxis, 0.25f, yAngle * 0.5f + new Degree(90), remainingYAngle);
}
else
{
Gizmos.Color = Color.Green;
Gizmos.Transform = joint.SceneObject.WorldTransform;
Gizmos.DrawWireDisc(Vector3.Zero, Vector3.ZAxis, 0.25f);
Gizmos.DrawWireDisc(Vector3.Zero, Vector3.YAxis, 0.25f);
}
}
/// <summary>
/// Creates a quaternion from axis/angle rotation.
/// </summary>
/// <param name="axis">Axis around which the rotation is performed.</param>
/// <param name="angle">Amount of rotation.</param>
/// <returns>Quaternion that rotates an object around the specified axis for the specified amount.</returns>
public static Quaternion FromAxisAngle(Vector3 axis, Degree angle)
{
Quaternion quat;
float halfAngle = (float)(0.5f*angle.Radians);
float sin = (float)MathEx.Sin(halfAngle);
quat.w = (float)MathEx.Cos(halfAngle);
quat.x = sin * axis.x;
quat.y = sin * axis.y;
quat.z = sin * axis.z;
return quat;
}
private static extern void Internal_SetFieldOfView(IntPtr instance, ref Degree value);
private static void DrawHingeJoint(HingeJoint joint)
{
Vector3 target = GetAnchor(joint, JointBody.Target);
Vector3 anchor = GetAnchor(joint, JointBody.Anchor);
Vector3 center = target;
Rigidbody rigidbody = joint.GetRigidbody(JointBody.Target);
if (rigidbody != null)
center = rigidbody.SceneObject.Position;
Gizmos.Color = Color.White;
Gizmos.DrawSphere(center, 0.05f);
Gizmos.Color = Color.Yellow;
Gizmos.DrawSphere(target, 0.05f);
Gizmos.DrawSphere(anchor, 0.05f);
Gizmos.Color = Color.Green;
Gizmos.DrawLine(target, center);
const float radius = 0.25f;
const float height = 0.5f;
if (joint.EnableLimit)
{
Gizmos.Transform = joint.SceneObject.WorldTransform;
LimitAngularRange limit = joint.Limit;
Action<float> drawLimitedArc = x =>
{
Degree lower = MathEx.WrapAngle(limit.Lower);
Degree upper = MathEx.WrapAngle(limit.Upper);
lower = MathEx.Min(lower, upper);
upper = MathEx.Max(upper, lower);
// Arc zero to lower limit
Gizmos.Color = Color.Red;
Gizmos.DrawWireArc(Vector3.XAxis * x, Vector3.XAxis, radius, new Degree(0.0f), lower);
// Arc lower to upper limit
Degree validRange = upper - lower;
Gizmos.Color = Color.Green;
Gizmos.DrawWireArc(Vector3.XAxis * x, Vector3.XAxis, radius, lower, validRange);
// Arc upper to full circle
Degree remainingRange = new Degree(360) - upper;
Gizmos.Color = Color.Red;
Gizmos.DrawWireArc(Vector3.XAxis * x, Vector3.XAxis, radius, upper, remainingRange);
};
drawLimitedArc(-height);
drawLimitedArc(height);
}
else
{
Gizmos.Color = Color.Green;
Gizmos.Transform = joint.SceneObject.WorldTransform;
Gizmos.DrawWireDisc(Vector3.XAxis * -height, Vector3.XAxis, radius);
Gizmos.DrawWireDisc(Vector3.XAxis * height, Vector3.XAxis, radius);
}
Vector3[] lineStartPoints = new Vector3[4];
lineStartPoints[0] = new Vector3(-height, radius, 0);
lineStartPoints[1] = new Vector3(-height, -radius, 0);
lineStartPoints[2] = new Vector3(-height, 0, radius);
lineStartPoints[3] = new Vector3(-height, 0, -radius);
Vector3[] lineEndPoints = new Vector3[4];
lineEndPoints[0] = new Vector3(height, radius, 0);
lineEndPoints[1] = new Vector3(height, -radius, 0);
lineEndPoints[2] = new Vector3(height, 0, radius);
lineEndPoints[3] = new Vector3(height, 0, -radius);
Gizmos.Color = Color.Green;
for (int i = 0; i < 4; i++)
Gizmos.DrawLine(lineStartPoints[i], lineEndPoints[i]);
}
/// <summary>
/// Converts the quaternion rotation into axis/angle rotation.
/// </summary>
/// <param name="rotation">Quaternion to convert.</param>
/// <param name="axis">Axis around which the rotation is performed.</param>
/// <param name="angle">Amount of rotation.</param>
public static void ToAxisAngle(Quaternion rotation, out Vector3 axis, out Degree angle)
{
rotation.ToAxisAngle(out axis, out angle);
}
/// <summary>
/// Applies the animation target state depending on the interpolation parameter. <see cref="SetState"/>.
/// </summary>
/// <param name="t">Interpolation parameter ranging [0, 1] that interpolated between the start state and the
/// target state.</param>
private void ApplyState(float t)
{
animation.Update(t);
SceneObject.Position = animation.State.Position;
SceneObject.Rotation = animation.State.Rotation;
frustumWidth = animation.State.FrustumWidth;
Vector3 eulerAngles = SceneObject.Rotation.ToEuler();
pitch = (Degree)eulerAngles.x;
yaw = (Degree)eulerAngles.y;
Degree FOV = (Degree)(1.0f - animation.State.OrtographicPct)*FieldOfView;
if (FOV < (Degree)5.0f)
{
camera.ProjectionType = ProjectionType.Orthographic;
camera.OrthoHeight = frustumWidth * 0.5f / camera.AspectRatio;
}
else
{
camera.ProjectionType = ProjectionType.Perspective;
camera.FieldOfView = FOV;
}
// Note: Consider having a global setting for near/far planes as changing it here might confuse the user
float distance = CalcDistanceForFrustumWidth(frustumWidth);
if (distance < 1)
{
camera.NearClipPlane = 0.005f;
camera.FarClipPlane = 1000f;
}
if (distance < 100)
{
camera.NearClipPlane = 0.05f;
camera.FarClipPlane = 2500f;
}
else if (distance < 1000)
{
camera.NearClipPlane = 0.5f;
camera.FarClipPlane = 10000f;
}
else
{
camera.NearClipPlane = 5.0f;
camera.FarClipPlane = 1000000f;
}
}
/// <summary>
/// Rotates around local Y axis.
/// </summary>
/// <param name="angle">Angle to rotate by.</param>
public void Yaw(Degree angle)
{
Radian radianAngle = angle;
Internal_Yaw(mCachedPtr, ref radianAngle);
}
/// <summary>
/// Rotates around local X axis.
/// </summary>
/// <param name="angle">Angle to rotate by.</param>
public void Pitch(Degree angle)
{
Radian radianAngle = angle;
Internal_Pitch(mCachedPtr, ref radianAngle);
}
/// <summary>
/// Rotates around local Z axis.
/// </summary>
/// <param name="angle">Angle to rotate by.</param>
public void Roll(Degree angle)
{
Radian radianAngle = angle;
Internal_Roll(mCachedPtr, ref radianAngle);
}
/// <summary>
/// Creates a new radian value.
/// </summary>
/// <param name="d">Value in degrees.</param>
public Radian(Degree d)
{
this.value = d.Radians;
}
private static extern void Internal_SetFieldOfView(IntPtr instance, ref Degree value);
/// <summary>
/// Converts the quaternion rotation into axis/angle rotation.
/// </summary>
/// <param name="axis">Axis around which the rotation is performed.</param>
/// <param name="angle">Amount of rotation.</param>
public void ToAxisAngle(out Vector3 axis, out Degree angle)
{
float fSqrLength = x*x+y*y+z*z;
if (fSqrLength > 0.0f)
{
angle = 2.0f * MathEx.Acos(w);
float fInvLength = MathEx.InvSqrt(fSqrLength);
axis.x = x*fInvLength;
axis.y = y*fInvLength;
axis.z = z*fInvLength;
}
else
{
// Angle is 0, so any axis will do
angle = (Degree)0.0f;
axis.x = 1.0f;
axis.y = 0.0f;
axis.z = 0.0f;
}
}
/// <summary>
/// Enables or disables a cut-off plane that can allow the disc to be intersected with only in a 180 degree arc.
/// </summary>
/// <param name="angle">Angle at which to start the cut-off. Points on the dist at the specified angle and the next
/// 180 degrees won't be interactable.</param>
/// <param name="enabled">Should the cutoff plane be enabled or disabled.</param>
public void SetCutoffPlane(Degree angle, bool enabled)
{
Internal_SetCutoffPlane(mCachedPtr, angle.Degrees, enabled);
}
/// <summary>
/// Snaps an angle value to the specified increments.
/// </summary>
/// <param name="value">Value to snap.</param>
/// <param name="snapAmount">Increment to which to snap the value to.</param>
/// <returns>Value snapped to the provided increments.</returns>
public static Degree SnapValue(Degree value, Degree snapAmount)
{
return (Degree)SnapValue(value.Degrees, snapAmount.Degrees);
}
/// <summary>
/// Creates a new radian value.
/// </summary>
/// <param name="d">Value in degrees.</param>
public Radian(Degree d)
{
this.value = d.Radians;
}
private void OnUpdate()
{
bool isOrtographic = camera.ProjectionType == ProjectionType.Orthographic;
if (inputEnabled)
{
bool goingForward = VirtualInput.IsButtonHeld(moveForwardBtn);
bool goingBack = VirtualInput.IsButtonHeld(moveBackwardBtn);
bool goingLeft = VirtualInput.IsButtonHeld(moveLeftBtn);
bool goingRight = VirtualInput.IsButtonHeld(moveRightBtn);
bool goingUp = VirtualInput.IsButtonHeld(moveUpBtn);
bool goingDown = VirtualInput.IsButtonHeld(moveDownBtn);
bool fastMove = VirtualInput.IsButtonHeld(fastMoveBtn);
bool camActive = VirtualInput.IsButtonHeld(activeBtn);
bool isPanning = VirtualInput.IsButtonHeld(panBtn);
bool hideCursor = camActive || isPanning;
if (hideCursor != lastButtonState)
{
if (hideCursor)
{
Cursor.Hide();
Rect2I clipRect;
clipRect.x = Input.PointerPosition.x - 2;
clipRect.y = Input.PointerPosition.y - 2;
clipRect.width = 4;
clipRect.height = 4;
Cursor.ClipToRect(clipRect);
}
else
{
Cursor.Show();
Cursor.ClipDisable();
}
lastButtonState = hideCursor;
}
float frameDelta = Time.FrameDelta;
if (camActive)
{
float horzValue = VirtualInput.GetAxisValue(horizontalAxis);
float vertValue = VirtualInput.GetAxisValue(verticalAxis);
float rotationAmount = RotationalSpeed * EditorSettings.MouseSensitivity * frameDelta;
yaw += new Degree(horzValue * rotationAmount);
pitch += new Degree(vertValue * rotationAmount);
yaw = MathEx.WrapAngle(yaw);
pitch = MathEx.WrapAngle(pitch);
Quaternion yRot = Quaternion.FromAxisAngle(Vector3.YAxis, yaw);
Quaternion xRot = Quaternion.FromAxisAngle(Vector3.XAxis, pitch);
Quaternion camRot = yRot*xRot;
camRot.Normalize();
SceneObject.Rotation = camRot;
// Handle movement using movement keys
Vector3 direction = Vector3.Zero;
if (goingForward) direction += SceneObject.Forward;
if (goingBack) direction -= SceneObject.Forward;
if (goingRight) direction += SceneObject.Right;
if (goingLeft) direction -= SceneObject.Right;
if (goingUp) direction += SceneObject.Up;
if (goingDown) direction -= SceneObject.Up;
if (direction.SqrdLength != 0)
{
direction.Normalize();
float multiplier = 1.0f;
if (fastMove)
multiplier = FastModeMultiplier;
currentSpeed = MathEx.Clamp(currentSpeed + Acceleration*frameDelta, StartSpeed, TopSpeed);
currentSpeed *= multiplier;
}
else
{
currentSpeed = 0.0f;
}
const float tooSmall = 0.0001f;
if (currentSpeed > tooSmall)
{
Vector3 velocity = direction*currentSpeed;
SceneObject.Move(velocity*frameDelta);
}
}
// Pan
if (isPanning)
{
float horzValue = VirtualInput.GetAxisValue(horizontalAxis);
float vertValue = VirtualInput.GetAxisValue(verticalAxis);
Vector3 direction = new Vector3(horzValue, -vertValue, 0.0f);
direction = camera.SceneObject.Rotation.Rotate(direction);
SceneObject.Move(direction*PanSpeed*frameDelta);
}
}
else
{
Cursor.Show();
Cursor.ClipDisable();
}
SceneWindow sceneWindow = EditorWindow.GetWindow<SceneWindow>();
if (sceneWindow.Active)
{
Rect2I bounds = sceneWindow.Bounds;
// Move using scroll wheel
if (bounds.Contains(Input.PointerPosition))
{
float scrollAmount = VirtualInput.GetAxisValue(scrollAxis);
if (!isOrtographic)
{
SceneObject.Move(SceneObject.Forward*scrollAmount*ScrollSpeed);
}
else
{
float orthoHeight = MathEx.Max(1.0f, camera.OrthoHeight - scrollAmount);
camera.OrthoHeight = orthoHeight;
}
}
}
UpdateAnim();
}
/// <summary>
/// Converts an orthonormal matrix to axis angle representation.
/// </summary>
/// <param name="axis">Axis around which the rotation is performed.</param>
/// <param name="angle">Amount of rotation.</param>
public void ToAxisAngle(out Vector3 axis, out Degree angle)
{
float trace = m00 + m11 + m22;
float cos = 0.5f * (trace - 1.0f);
Radian radians = MathEx.Acos(cos); // In [0, PI]
angle = radians.Degrees;
if (radians > 0.0f)
{
if (radians < MathEx.Pi)
{
axis.x = m21 - m12;
axis.y = m02 - m20;
axis.z = m10 - m01;
axis.Normalize();
}
else
{
// Angle is PI
float halfInverse;
if (m00 >= m11)
{
// r00 >= r11
if (m00 >= m22)
{
// r00 is maximum diagonal term
axis.x = 0.5f * MathEx.Sqrt(m00 - m11 - m22 + 1.0f);
halfInverse = 0.5f / axis.x;
axis.y = halfInverse * m01;
axis.z = halfInverse * m02;
}
else
{
// r22 is maximum diagonal term
axis.z = 0.5f * MathEx.Sqrt(m22 - m00 - m11 + 1.0f);
halfInverse = 0.5f / axis.z;
axis.x = halfInverse * m02;
axis.y = halfInverse * m12;
}
}
else
{
// r11 > r00
if (m11 >= m22)
{
// r11 is maximum diagonal term
axis.y = 0.5f * MathEx.Sqrt(m11 - m00 - m22 + 1.0f);
halfInverse = 0.5f / axis.y;
axis.x = halfInverse * m01;
axis.z = halfInverse * m12;
}
else
{
// r22 is maximum diagonal term
axis.z = 0.5f * MathEx.Sqrt(m22 - m00 - m11 + 1.0f);
halfInverse = 0.5f / axis.z;
axis.x = halfInverse * m02;
axis.y = halfInverse * m12;
}
}
}
}
else
{
// The angle is 0 and the matrix is the identity. Any axis will
// work, so just use the x-axis.
axis.x = 1.0f;
axis.y = 0.0f;
axis.z = 0.0f;
}
}
