private void RotateCameraAroundLookDir(Point currentPosition, Vector3D currentPosition3D)
{
Vector3D curPos3DFlat = currentPosition3D;
curPos3DFlat.Z = 0;
curPos3DFlat.Normalize();
Vector3D prevPos3DFlat = _previousPosition3D;
prevPos3DFlat.Z = 0;
prevPos3DFlat.Normalize();
Vector3D axis = Vector3D.CrossProduct(prevPos3DFlat, curPos3DFlat);
double angle = Vector3D.AngleBetween(prevPos3DFlat, curPos3DFlat);
// 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);
Vector3D[] vectors = new Vector3D[] { _camera.UpDirection, _camera.LookDirection };
deltaRotation.GetRotatedVector(vectors);
// Apply the changes
_camera.UpDirection = vectors[0];
_camera.LookDirection = vectors[1];
}
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);
#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 * _orbitRadius.Value;
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];
}
private void RandomCamera_Click(object sender, RoutedEventArgs e)
{
try
{
// Position
Vector3D position = Math3D.GetRandomVector_Spherical(CAMERADISTANCE / 2, CAMERADISTANCE * 2);
// Look Direction
Vector3D lookDirection = position * -1;
Vector3D rotateAxis = Math3D.GetRandomVector_Cone(Math3D.GetArbitraryOrhonganal(lookDirection), 20);
Quaternion rotate = new Quaternion(rotateAxis, Math1D.GetNearZeroValue(20));
lookDirection = rotate.GetRotatedVector(lookDirection);
// Up Vector
Vector3D up = Math3D.GetArbitraryOrhonganal(lookDirection);
// Commit
_camera.Position = position.ToPoint();
_camera.LookDirection = lookDirection;
_camera.UpDirection = up;
}
catch (Exception ex)
{
MessageBox.Show(ex.ToString(), this.Title, MessageBoxButton.OK, MessageBoxImage.Error);
}
}
private void Update_Direction_Constant(double elapsedTime)
{
Quaternion delta = new Quaternion(_angVel.Axis, _angVel.Angle * this.Speed_Orientation * elapsedTime);
this.Direction = delta.GetRotatedVector(this.Direction);
}
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
}
private void addJoined1_AddJoint(object sender, AddJoinedBodiesArgs e)
{
try
{
// Figure out the centerpoint
Point3D centerPoint = Math3D.GetRandomVector_Spherical(CREATEOBJECTBOUNDRY).ToPoint();
// Get a random rotation
//TODO: Figure out why it fails when I give it a rotation
//Quaternion finalRotation = new Quaternion(Math3D.GetRandomVectorSpherical(_rand, 10), Math3D.GetNearZeroValue(_rand, 360d));
Quaternion finalRotation = new Quaternion(new Vector3D(0, 0, 1), 0);
// All bodies will be the same color
Color color = UtilityWPF.GetRandomColor(64, 192);
Body[] bodies = null;
#region Get Bodies
switch (e.JointType)
{
case AddJointType.BallAndSocket:
case AddJointType.Hinge:
case AddJointType.Slider:
case AddJointType.Corkscrew:
case AddJointType.UniversalJoint:
bodies = new Body[2];
GetJointBodyPair(out bodies[0], out bodies[1], e.Body1Type, e.Body2Type, centerPoint, finalRotation, e.SeparationDistance, color);
break;
case AddJointType.UpVector:
case AddJointType.Multi_BallAndChain:
case AddJointType.Multi_Tetrahedron:
MessageBox.Show("finish this");
return;
default:
throw new ApplicationException("Unknown AddJointType: " + e.JointType.ToString());
}
#endregion
_bodySets.Add(bodies);
#region Setup Joint
Vector3D directionAlong = finalRotation.GetRotatedVector(new Vector3D(1, 0, 0));
Vector3D directionOrth1 = finalRotation.GetRotatedVector(new Vector3D(0, 1, 0));
Vector3D directionOrth2 = finalRotation.GetRotatedVector(new Vector3D(0, 0, 1));
switch (e.JointType)
{
case AddJointType.BallAndSocket:
#region BallAndSocket
JointBallAndSocket ballAndSocket = JointBallAndSocket.CreateBallAndSocket(_world, centerPoint, bodies[0], bodies[1]);
ballAndSocket.ShouldLinkedBodiesCollideEachOther = true;
//TODO: Let the user define these limits
//ballAndSocket.SetConeLimits();
#endregion
break;
case AddJointType.Hinge:
#region Hinge
JointHinge hinge = JointHinge.CreateHinge(_world, centerPoint, directionOrth1, bodies[0], bodies[1]);
hinge.ShouldLinkedBodiesCollideEachOther = true;
#endregion
break;
case AddJointType.Slider:
#region Slider
JointSlider slider = JointSlider.CreateSlider(_world, centerPoint, directionAlong, bodies[0], bodies[1]);
slider.ShouldLinkedBodiesCollideEachOther = true;
#endregion
break;
case AddJointType.Corkscrew:
#region Corkscrew
JointCorkscrew corkscrew = JointCorkscrew.CreateCorkscrew(_world, centerPoint, directionAlong, bodies[0], bodies[1]);
corkscrew.ShouldLinkedBodiesCollideEachOther = true;
#endregion
break;
case AddJointType.UniversalJoint:
#region UniversalJoint
JointUniversal uJoint = JointUniversal.CreateUniversal(_world, centerPoint, directionOrth1, directionOrth2, bodies[0], bodies[1]);
uJoint.ShouldLinkedBodiesCollideEachOther = true;
#endregion
break;
default:
throw new ApplicationException("Unexpected AddJointType: " + e.JointType.ToString());
}
#endregion
BodiesAdded(bodies);
}
catch (Exception ex)
{
MessageBox.Show(ex.ToString(), this.Title, MessageBoxButton.OK, MessageBoxImage.Error);
}
}
private static Quaternion GetDelta(Quaternion from, Quaternion to)
{
DoubleVector fromVect = from.GetRotatedVector(new DoubleVector(1, 0, 0, 0, 1, 0));
DoubleVector toVect = to.GetRotatedVector(new DoubleVector(1, 0, 0, 0, 1, 0));
return Math3D.GetRotation(fromVect, toVect);
}
