public static TwistConstraint MakeFromRadians(CartesianAxis twistAxis, Vector3 min, Vector3 max)
{
return(MakeFromRadians(
min[(int)twistAxis],
max[(int)twistAxis]
));
}
private void Solve(RigidBoneSystem boneSystem, InverseKinematicsGoal goal, RigidBoneSystemInputs inputs)
{
var bone = boneSystem.BonesByName[boneName];
//get bone transforms with the current bone rotation zeroed out
inputs.Rotations[bone.Index] = TwistSwing.Zero;
var boneTransforms = boneSystem.GetBoneTransforms(inputs);
var boneTransform = boneTransforms[bone.Index];
var worldSourcePosition = boneTransforms[goal.SourceBone.Index].Transform(goal.SourceBone.CenterPoint + goal.UnposedSourcePosition);
var worldTargetPosition = goal.TargetPosition;
var worldCenterPosition = boneTransform.Transform(bone.CenterPoint);
var worldSourceDirection = Vector3.Normalize(worldSourcePosition - worldCenterPosition);
var worldTargetDirection = Vector3.Normalize(worldTargetPosition - worldCenterPosition);
//transform source and target to bone's oriented space
var parentTotalRotation = bone.Parent != null ? boneTransforms[bone.Parent.Index].Rotation : Quaternion.Identity;
var orientedSpaceToWorldTransform = bone.OrientationSpace.Orientation.Chain(parentTotalRotation);
var worldToOrientatedSpaceTransform = Quaternion.Invert(orientedSpaceToWorldTransform);
var orientedSourceDirection = Vector3.Transform(worldSourceDirection, worldToOrientatedSpaceTransform);
var orientedTargetDirection = Vector3.Transform(worldTargetDirection, worldToOrientatedSpaceTransform);
CartesianAxis twistAxis = (CartesianAxis)bone.RotationOrder.primaryAxis;
var newOrientedRotation = Swing.FromTo(twistAxis, orientedSourceDirection, orientedTargetDirection);
bone.SetOrientedSpaceRotation(inputs, new TwistSwing(Twist.Zero, newOrientedRotation), true);
}
public static float Get(this Vector3 v, CartesianAxis axis)
{
switch (axis)
{
case CartesianAxis.Zneg:
return(-v.z);
case CartesianAxis.Yneg:
return(-v.y);
case CartesianAxis.Xneg:
return(-v.x);
case CartesianAxis.X:
return(v.x);
case CartesianAxis.Y:
return(v.y);
case CartesianAxis.Z:
return(v.z);
default:
return(0f);
}
}
public static Vector3 GetAxis(this Transform trans, CartesianAxis axis, bool inLocalSpace)
{
if (trans == null)
{
throw new System.ArgumentNullException("trans");
}
Vector3 v = Vector3.zero;
switch (axis)
{
case CartesianAxis.X:
v = trans.right;
break;
case CartesianAxis.Y:
v = trans.up;
break;
case CartesianAxis.Z:
v = trans.forward;
break;
}
return((inLocalSpace) ? trans.InverseTransformDirection(v) : v);
}
public static TwistSwingConstraint MakeFromRadians(CartesianAxis twistAxis, Vector3 min, Vector3 max)
{
var twistConstraint = TwistConstraint.MakeFromRadians(twistAxis, min, max);
var swingConstraint = SwingConstraint.MakeFromRadians(twistAxis, min, max);
return(new TwistSwingConstraint(twistConstraint, swingConstraint));
}
public static Swing FromTo(CartesianAxis twistAxis, Vector3 from, Vector3 to)
{
/*
* This function is not optimized.
* I should write an optimized version if I need to use FromTo in production.
*/
DebugUtilities.AssertIsUnit(from);
DebugUtilities.AssertIsUnit(to);
float fromX = from[((int)twistAxis + 0) % 3];
float fromY = from[((int)twistAxis + 1) % 3];
float fromZ = from[((int)twistAxis + 2) % 3];
float toX = to[((int)twistAxis + 0) % 3];
float toY = to[((int)twistAxis + 1) % 3];
float toZ = to[((int)twistAxis + 2) % 3];
Vector2 axis = Vector2.Normalize(new Vector2(fromZ - toZ, toY - fromY));
float projectionLength = axis.X * fromY + axis.Y * fromZ;
Vector2 projection = axis * projectionLength; //by construction, projection onto axis is same for from and to
Vector3 fromRejection = new Vector3(fromY - projection.X, fromZ - projection.Y, fromX);
Vector3 toRejection = new Vector3(toY - projection.X, toZ - projection.Y, toX);
Vector3 rejectionCross = Vector3.Cross(fromRejection, toRejection);
float rejectionDot = Vector3.Dot(fromRejection, toRejection);
float angle = (float)Atan2(axis.X * rejectionCross.X + axis.Y * rejectionCross.Y, rejectionDot);
return(Swing.AxisAngle(axis.X, axis.Y, angle));
}
public static Vector3 Set(this Vector3 v, CartesianAxis axis, float value)
{
switch (axis)
{
case CartesianAxis.Zneg:
v.z = -value;
return(v);
case CartesianAxis.Yneg:
v.y = -value;
return(v);
case CartesianAxis.Xneg:
v.x = -value;
return(v);
case CartesianAxis.X:
v.x = value;
return(v);
case CartesianAxis.Y:
v.y = value;
return(v);
case CartesianAxis.Z:
v.z = value;
return(v);
default:
return(v);
}
}
public static Vector3 GetAxis(this Transform trans, CartesianAxis axis)
{
if (trans == null)
{
throw new System.ArgumentNullException("trans");
}
switch (axis)
{
case CartesianAxis.Xneg:
return(-trans.right);
case CartesianAxis.Yneg:
return(-trans.up);
case CartesianAxis.Zneg:
return(-trans.forward);
case CartesianAxis.X:
return(trans.right);
case CartesianAxis.Y:
return(trans.up);
case CartesianAxis.Z:
return(trans.forward);
}
return(Vector3.zero);
}
public static TwistSwing Decompose(CartesianAxis twistAxis, Quaternion q)
{
DebugUtilities.AssertIsUnit(q);
float w = q.W;
float x = q[(int)twistAxis];
float y = q[((int)twistAxis + 1) % 3];
float z = q[((int)twistAxis + 2) % 3];
float swingW = (float)Sqrt(Sqr(w) + Sqr(x));
float twistW, twistZ;
if (swingW != 0)
{
twistW = w / swingW;
twistZ = x / swingW;
}
else
{
//if swingW is 0, then there isn't a unique decomposition, so I'll arbitrarily assume no twist
twistW = 1;
twistZ = 0;
}
float swingY = twistW * y - twistZ * z;
float swingZ = twistW * z + twistZ * y;
var twist = new Twist(Sign(twistW) * twistZ);
var swing = new Swing(swingY, swingZ);
return(new TwistSwing(twist, swing));
}
public static Vector3 GetUnitVector(CartesianAxis axis)
{
switch (axis)
{
case CartesianAxis.Zneg:
return(Vector3.back);
case CartesianAxis.Yneg:
return(Vector3.down);
case CartesianAxis.Xneg:
return(Vector3.right);
case CartesianAxis.X:
return(Vector3.right);
case CartesianAxis.Y:
return(Vector3.up);
case CartesianAxis.Z:
return(Vector3.forward);
default:
return(Vector3.zero);
}
}
public static void Set(ref Vector3 v, CartesianAxis axis, float value)
{
switch (axis)
{
case CartesianAxis.Zneg:
v.z = -value;
break;
case CartesianAxis.Yneg:
v.y = -value;
break;
case CartesianAxis.Xneg:
v.x = -value;
break;
case CartesianAxis.X:
v.x = value;
break;
case CartesianAxis.Y:
v.y = value;
break;
case CartesianAxis.Z:
v.z = value;
break;
}
}
public Quaternion AsQuaternion(CartesianAxis twistAxis)
{
Quaternion q = default(Quaternion);
q[(int)twistAxis] = X;
q.W = W;
return(q);
}
public Vector3 GetRotationAxis(CartesianAxis twistAxis)
{
Vector3 axis = default(Vector3);
axis[((int)twistAxis + 1) % 3] = Y;
axis[((int)twistAxis + 2) % 3] = Z;
return(Vector3.Normalize(axis));
}
public Quaternion AsQuaternion(CartesianAxis twistAxis)
{
Quaternion q = default(Quaternion);
q[((int)twistAxis + 1) % 3] = Y;
q[((int)twistAxis + 2) % 3] = Z;
q.W = W;
return(q);
}
public static Swing To(CartesianAxis twistAxis, Vector3 to)
{
DebugUtilities.AssertIsUnit(to);
float toX = to[((int)twistAxis + 0) % 3];
float toY = to[((int)twistAxis + 1) % 3];
float toZ = to[((int)twistAxis + 2) % 3];
/*
* To reconstruct the swing quaternion, we need to calculate:
* <y, z> = Sin[angle / 2] * rotation-axis
* w = Cos[angle / 2]
*
* We know:
* Cos[angle]
* = Dot[twist-axis, to]
* = toX
*
* rotation-axis
* = Normalize[Cross[twist-axis, to]]
* = Normalize[<-toZ, toX>]
* = <-toZ, toX> / Sqrt[toX^2 + toZ^2]
* = <-toZ, toX> / Sqrt[1 - toX^2]
*
* So:
* w = Cos[angle / 2]
* = Sqrt[(1 + Cos[angle]) / 2] (half-angle trig identity)
* = Sqrt[(1 + toX) / 2]
*
* <y,z>
* = Sin[angle / 2] * rotation-axis
* = Sqrt[(1 - Cos[angle]) / 2] * rotation-axis (half-angle trig identity)
* = Sqrt[(1 - toX) / 2] * rotation-axis
* = Sqrt[(1 - toX) / 2] / Sqrt[1 - toX^2] * <-toZ, toY>
* = Sqrt[(1 - toX) / (2 * (1 - toX^2))] * <-toZ, toY>
* = Sqrt[(1 - toX) / (2 * (1 - toX) * (1 + toX))] * <-toZ, toY>
* = Sqrt[1 / (2 * (1 + toX))] * <-toZ, toY>
* = 1 / (2 * w) * <-toZ, toY>
*/
float ww = (1 + toX);
float wy = -toZ;
float wz = +toY;
if (ww < MathUtil.ZeroTolerance)
{
// This is a 180 degree swing (W = 0) so X and Y don't have a unique value
// I'll arbitrarily use:
return(new Swing(1, 0));
}
return(Swing.MakeUnitized(ww, wy, wz));
}
public Vector3 TransformTwistAxis(CartesianAxis twistAxis)
{
float x = 2 * WSquared - 1;
float y = 2 * W * Z;
float z = 2 * W * -Y;
Vector3 v = default(Vector3);
v[((int)twistAxis + 0) % 3] = x;
v[((int)twistAxis + 1) % 3] = y;
v[((int)twistAxis + 2) % 3] = z;
return(v);
}
public static Vector3 GetAxis(CartesianAxis axis)
{
switch (axis)
{
case CartesianAxis.X:
return Vector3.right;
case CartesianAxis.Y:
return Vector3.up;
case CartesianAxis.Z:
return Vector3.forward;
}
return Vector3.zero;
}
public static Vector3 GetAxis(this Transform trans, CartesianAxis axis)
{
if (trans == null) throw new System.ArgumentNullException("trans");
switch(axis)
{
case CartesianAxis.X:
return trans.right;
case CartesianAxis.Y:
return trans.up;
case CartesianAxis.Z:
return trans.forward;
}
return Vector3.zero;
}
public static Vector3 GetAxis(CartesianAxis axis)
{
switch (axis)
{
case CartesianAxis.X:
return(Vector3.right);
case CartesianAxis.Y:
return(Vector3.up);
case CartesianAxis.Z:
return(Vector3.forward);
}
return(Vector3.zero);
}
/// <summary>
/// Arranges the elements in a panel
/// </summary>
/// <param name="finalSize">final size of the panel.</param>
/// <returns>returns Size</returns>
public Size Arrange(Size finalSize)
{
Rect clientRect = new Rect(0, 0, finalSize.Width, finalSize.Height);
if (PolarAxis != null)
{
PolarAxis.ArrangeRect = clientRect;
PolarAxis.Measure(new Size(clientRect.Width, clientRect.Height));
PolarAxis.Arrange(clientRect);
Canvas.SetLeft(PolarAxis, clientRect.Left);
Canvas.SetTop(PolarAxis, clientRect.Top);
}
ChartAxis yAxis = this.CartesianAxis;
if (yAxis != null)
{
Point center = ChartLayoutUtils.GetCenter(clientRect);
CalculateCartesianArrangeRect(center, yAxis);
Rect rect = new Rect(this.CartesianAxis.ArrangeRect.Left,
this.CartesianAxis.ArrangeRect.Top,
this.CartesianAxis.ComputedDesiredSize.Width,
this.CartesianAxis.ComputedDesiredSize.Height);
if (CartesianAxis.PolarAngle != ChartPolarAngle.Rotate90 &&
(!yAxis.OpposedPosition ||
CartesianAxis.PolarAngle == ChartPolarAngle.Rotate180))
{
CartesianAxis.Measure(new Size(rect.Width, rect.Height));
}
else
{
CartesianAxis.Measure(new Size(rect.Left, rect.Height));
}
CartesianAxis.Arrange(rect);
Canvas.SetLeft(CartesianAxis, CartesianAxis.ArrangeRect.Left);
Canvas.SetTop(CartesianAxis, CartesianAxis.ArrangeRect.Top);
if (CartesianAxis.PolarAngle == ChartPolarAngle.Rotate90 ||
CartesianAxis.PolarAngle == ChartPolarAngle.Rotate180)
{
Area.InternalSecondaryAxis.IsInversed = !Area.InternalSecondaryAxis.IsInversed;
}
}
return(finalSize);
}
public Quaternion AsQuaternion(CartesianAxis twistAxis)
{
float twistX = twist.X;
float twistW = twist.W;
float swingY = swing.Y;
float swingZ = swing.Z;
float swingW = swing.W;
Quaternion q = default(Quaternion);
q[(int)twistAxis] = swingW * twistX;
q[((int)twistAxis + 1) % 3] = twistW * swingY + twistX * swingZ;
q[((int)twistAxis + 2) % 3] = twistW * swingZ - twistX * swingY;
q.W = swingW * twistW;
return(q);
}
public BasicGameScene(PhysicsGame physicsGame, Game xnaGame)
{
this.physicsGame = physicsGame;
this.xnaGame = xnaGame;
Mouse.SetPosition(xnaGame.Window.ClientBounds.Width / 2, xnaGame.Window.ClientBounds.Height / 2);
//Camera
CameraMoveable camera = new CameraMoveable(new Vector3(0, 8, 12), new Vector3(0, 0, 0), Vector3.Up, 0.01f, 4000.0f, (float)xnaGame.Window.ClientBounds.Width / (float)xnaGame.Window.ClientBounds.Height);
fpsCameraMover = new FPSCameraMover(xnaGame, camera);
physicsGame.ActiveCamera = camera;
//Cartesian Axis
cartesianAxis = new CartesianAxis(xnaGame.GraphicsDevice, 1000, Color.Red, Color.Black, Color.Green, physicsGame.ActiveCamera);
}
public Vector3 Transform(CartesianAxis twistAxis, Vector3 v)
{
float vx = v[((int)twistAxis + 0) % 3];
float vy = v[((int)twistAxis + 1) % 3];
float vz = v[((int)twistAxis + 2) % 3];
float w = W;
float c = 2 * (w * vx - z * vy + y * vz);
float rx = -vx + w * c;
float ry = +vy + z * c;
float rz = +vz - y * c;
Vector3 result = default(Vector3);
result[((int)twistAxis + 0) % 3] = rx;
result[((int)twistAxis + 1) % 3] = ry;
result[((int)twistAxis + 2) % 3] = rz;
return(result);
}
public static Vector3 GetAxis(this Transform trans, CartesianAxis axis, bool inLocalSpace)
{
if (trans == null) throw new System.ArgumentNullException("trans");
Vector3 v = Vector3.zero;
switch (axis)
{
case CartesianAxis.X:
v = trans.right;
break;
case CartesianAxis.Y:
v = trans.up;
break;
case CartesianAxis.Z:
v = trans.forward;
break;
}
return (inLocalSpace) ? trans.InverseTransformDirection(v) : v;
}
/// <summary>
/// Measures the elements in the panel
/// </summary>
/// <param name="availableSize">Available Size of the panel</param>
/// <returns></returns>
public Size Measure(Size availableSize)
{
isRadiusCalculating = true;
if (PolarAxis != null)
{
PolarAxis.ComputeDesiredSize(availableSize);
if (PolarAxis.EnableScrollBar)
{
PolarAxis.DisableScrollbar = true;
PolarAxis.EnableScrollBar = !PolarAxis.DisableScrollbar;
}
}
if (CartesianAxis != null)
{
if (CartesianAxis.EnableScrollBar)
{
CartesianAxis.DisableScrollbar = true;
CartesianAxis.EnableScrollBar = !CartesianAxis.DisableScrollbar;
}
CalculateSeriesRect(availableSize);
//Based on StartAngle cartesian axis has been measured
if (CartesianAxis.PolarAngle == ChartPolarAngle.Rotate0 ||
CartesianAxis.PolarAngle == ChartPolarAngle.Rotate180)
{
CartesianAxis.ComputeDesiredSize(new Size
((Math.Min(Area.SeriesClipRect.Width, Area.SeriesClipRect.Height) / 2),
Area.SeriesClipRect.Height));
}
else
{
CartesianAxis.ComputeDesiredSize(new Size(Area.SeriesClipRect.Width,
Math.Min(Area.SeriesClipRect.Width, Area.SeriesClipRect.Height) / 2));
}
}
isRadiusCalculating = false;
desiredSize = availableSize;
return(availableSize);
}
public static SwingConstraint MakeFromRadians(CartesianAxis twistAxis, Vector3 min, Vector3 max)
{
return(MakeFromRadians(
min[((int)twistAxis + 1) % 3], max[((int)twistAxis + 1) % 3],
min[((int)twistAxis + 2) % 3], max[((int)twistAxis + 2) % 3]));
}
public static Vector3 AsUnitVector(CartesianAxis axis)
{
return(UnitVectors[(int)axis]);
}
