private void OrbitCamera(Point currentPosition, Vector3D currentPosition3D)
{
#region Get Mouse Movement - Spherical
// Figure out a rotation axis and angle
Vector3D axis = Vector3D.CrossProduct(_previousPosition3D, currentPosition3D);
double angle = Vector3D.AngleBetween(_previousPosition3D, currentPosition3D);
// Quaterion will throw if this happens - sometimes we can get 3D positions that are very similar, so we
// avoid the throw by doing this check and just ignoring the event
if (axis.Length == 0)
{
return;
}
// Now need to rotate the axis into the camera's coords
// Get the camera's current view matrix.
Matrix3D viewMatrix = MathUtils.GetViewMatrix(_camera);
viewMatrix.Invert();
// Transform the trackball rotation axis relative to the camera orientation.
axis = viewMatrix.Transform(axis);
Quaternion deltaRotation = new Quaternion(axis, -angle);
Quaternion deltaRotationExternal = new Quaternion(axis, angle);
#endregion
// This can't be calculated each mose move. It causes a wobble when the look direction isn't pointed directly at the origin
//if (_orbitRadius == null)
//{
// _orbitRadius = OnGetOrbitRadius();
//}
// Figure out the offset in world coords
Vector3D lookLine = _camera.LookDirection;
lookLine.Normalize();
lookLine = lookLine * _camera.Position.ToVector().Length; //_orbitRadius.Value; // the camera is always pointed to the origin, so this shortcut works
Point3D orbitPointWorld = _camera.Position + lookLine;
// Get the opposite of the look line (the line from the orbit center to the camera's position)
Vector3D lookLineOpposite = lookLine * -1d;
// Rotate
Vector3D[] vectors = new Vector3D[] { lookLineOpposite, _camera.UpDirection, _camera.LookDirection };
deltaRotation.GetRotatedVector(vectors);
// Apply the changes
_camera.Position = orbitPointWorld + vectors[0];
_camera.UpDirection = vectors[1];
_camera.LookDirection = vectors[2];
_quaternion = _quaternion.ToUnit() * deltaRotationExternal.ToUnit();
_transform = new RotateTransform3D(new QuaternionRotation3D(_quaternion));
OnRotationChanged();
}
/// <summary>
/// This returns the current quaternion rotated by the delta
/// </summary>
/// <remarks>
/// This method is really simple, but I'm tired of trial and error with multiplication order every time I need
/// to rotate quaternions
/// </remarks>
public static Quaternion RotateBy(this Quaternion quaternion, Quaternion delta)
{
//return delta.ToUnit() * quaternion.ToUnit(); // this is the one that's backward (I think)
return quaternion.ToUnit() * delta.ToUnit();
}
private void GetJointBodyPair(out Body body1, out Body body2, AddJointBodyType bodyType1, AddJointBodyType bodyType2, Point3D centerPoint, Quaternion rotation, double separationDistance, Color color)
{
#region Body 1
double distanceToPermiter;
Quaternion localRotation;
GetJointBodyPairSprtOffset(out distanceToPermiter, out localRotation, bodyType1);
Vector3D offset = new Vector3D(distanceToPermiter + (separationDistance / 2d), 0, 0); // adding to the separation distance because I don't want centerpoint to centerpoint, I want edge to edge
offset = rotation.GetRotatedVector(offset);
Point3D shiftedCenter = centerPoint + offset;
localRotation = rotation.ToUnit() * localRotation.ToUnit();
RotateTransform3D finalRotation = new RotateTransform3D(new QuaternionRotation3D(localRotation));
body1 = GetJointBodyPairSprtBody(GetJointBodyPairSprtHullType(bodyType1), shiftedCenter, finalRotation, color);
#endregion
#region Body 2
GetJointBodyPairSprtOffset(out distanceToPermiter, out localRotation, bodyType2);
offset = new Vector3D(distanceToPermiter + (separationDistance / 2d), 0, 0);
offset = rotation.GetRotatedVector(offset);
shiftedCenter = centerPoint - offset; // subtracting instead of adding
localRotation = new Quaternion(new Vector3D(0, 0, 1), 180d).ToUnit() * rotation.ToUnit() * localRotation.ToUnit(); // throwing in an extra 180 degrees of spin
finalRotation = new RotateTransform3D(new QuaternionRotation3D(localRotation));
body2 = GetJointBodyPairSprtBody(GetJointBodyPairSprtHullType(bodyType2), shiftedCenter, finalRotation, color);
#endregion
}