public static void GetRotatedVector(this Quaternion quaternion, Point3D[] points)
{
Matrix3D matrix = new Matrix3D();
matrix.Rotate(quaternion);
MatrixTransform3D transform = new MatrixTransform3D(matrix);
transform.Transform(points);
}
public static System.Windows.Media.Media3D.Matrix3D SetTilt(
this System.Windows.Media.Media3D.Matrix3D matrix, double tilt)
{
Vector3D translation = new System.Windows.Media.Media3D.Vector3D(matrix.OffsetX, matrix.OffsetY, matrix.OffsetZ);
/*
* var orientation = matrix.GetOrientation();
* var spin = matrix.GetSpin();
*
* var newMatrix = new Matrix3D();
* newMatrix = newMatrix.RotateOTS(new Vector3D(orientation, tilt,spin));
* newMatrix.Translate(translation);
* return newMatrix;*/
// backoff translation
matrix.Translate(new System.Windows.Media.Media3D.Vector3D(-translation.X, -translation.Y, -translation.Z));
Vector3D localX = matrix.Transform(new System.Windows.Media.Media3D.Vector3D(1, 0, 0));
// backoff exiting tilt
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(localX, matrix.GetTilt() * -1.0));
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(localX, tilt));
// add back translation
matrix.Translate(translation);
return(matrix);
}
public static Matrix3D RotateXYZ(Matrix3D matrix, Vector3D rotationsXYZ)
{
matrix.Rotate(new Quaternion(new Vector3D(0, 0, 1), rotationsXYZ.Z));
var localY = matrix.Transform(new Vector3D(0, 1, 0));
matrix.Rotate(new Quaternion(localY, rotationsXYZ.Y));
var localX = matrix.Transform(new Vector3D(1, 0, 0));
matrix.Rotate(new Quaternion(localX, rotationsXYZ.X));
return matrix;
}
public static Matrix3D RotateXYZ(Matrix3D matrix, double xRotationDegrees, double yRotationDegrees, double zRotationDegrees)
{
matrix.Rotate(new Quaternion(new Vector3D(1, 0, 0), xRotationDegrees));
var localY = matrix.Transform(new Vector3D(0, 1, 0));
matrix.Rotate(new Quaternion(localY, yRotationDegrees));
var localZ = matrix.Transform(new Vector3D(0, 0, 1));
matrix.Rotate(new Quaternion(localZ, zRotationDegrees));
return matrix;
}
public static System.Windows.Media.Media3D.Matrix3D SetOrientation(this System.Windows.Media.Media3D.Matrix3D matrix, double orientationDegrees)
{
Vector3D translation = new System.Windows.Media.Media3D.Vector3D(matrix.OffsetX, matrix.OffsetY, matrix.OffsetZ);
// backoff translation
matrix.Translate(new System.Windows.Media.Media3D.Vector3D(-translation.X, -translation.Y, -translation.Z));
// back off any existing Z rotation
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(new System.Windows.Media.Media3D.Vector3D(0, 0, 1), matrix.GetOrientation() * -1.0));
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(new System.Windows.Media.Media3D.Vector3D(0, 0, 1), orientationDegrees));
// add back translation
matrix.Translate(translation);
return(matrix);
}
public Transform3DGroup CreateTransformGroup(Halo3.ObjectChunk placedObject)
{
var transformGroup = new Transform3DGroup();
float yaw, pitch, roll;
Core.Helpers.VectorMath.Convert.ToYawPitchRoll(
placedObject.SpawnPosition.Right,
placedObject.SpawnPosition.Forward,
placedObject.SpawnPosition.Up,
out yaw,
out pitch,
out roll);
// For some reason you have to swag the roll and yaw.
var swag = Microsoft.Xna.Framework.Quaternion.CreateFromYawPitchRoll(roll, pitch, yaw);
// Apply 3D Matrix
var matrix = new Matrix3D();
matrix.Rotate(new Quaternion(swag.X, swag.Y, swag.Z, swag.W));
matrix.OffsetX = placedObject.SpawnCoordinates.X;
matrix.OffsetY = placedObject.SpawnCoordinates.Y;
matrix.OffsetZ = placedObject.SpawnCoordinates.Z;
// TODO: F**K THIS VALUE
// TODO: AND F**K BUNGIE
//matrix.Prepend(new Matrix3D
// {
// OffsetX = 0,
// OffsetY = 0,
// OffsetZ = 0
// });
transformGroup.Children.Add(new MatrixTransform3D(matrix));
return transformGroup;
}
public static System.Windows.Media.Media3D.Matrix3D SetSpin(
this System.Windows.Media.Media3D.Matrix3D matrix, double spin)
{
Vector3D translation = new System.Windows.Media.Media3D.Vector3D(matrix.OffsetX, matrix.OffsetY, matrix.OffsetZ);
// backoff translation
matrix.Translate(new System.Windows.Media.Media3D.Vector3D(-translation.X, -translation.Y, -translation.Z));
Vector3D localZ = matrix.Transform(new System.Windows.Media.Media3D.Vector3D(0, 0, 1));
// backoff existing spin
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(localZ, matrix.GetSpin() * -1.0));
matrix.Rotate(new System.Windows.Media.Media3D.Quaternion(localZ, spin));
// add back translation
matrix.Translate(translation);
return(matrix);
}
/// <summary>
/// Rotates the double vector around the angle in degrees
/// </summary>
public DoubleVector GetRotatedVector(Vector3D axis, double angle)
{
Matrix3D matrix = new Matrix3D();
matrix.Rotate(new Quaternion(axis, angle));
MatrixTransform3D transform = new MatrixTransform3D(matrix);
return new DoubleVector(transform.Transform(this.Standard), transform.Transform(this.Orth));
}
public void Rotate(Vector3D rotationAmount)
{
var rotationAroundX = new Matrix3D();
rotationAroundX.Rotate(new Quaternion(Right, Trig.DegreeToRadian(rotationAmount.Y)));
var rotationAroundY = new Matrix3D();
rotationAroundY.Rotate(new Quaternion(Up, Trig.DegreeToRadian(rotationAmount.X)));
var rotationMatrix = Matrix3D.Multiply(rotationAroundX, rotationAroundY);
Direction = Vector3D.Multiply(Direction, rotationMatrix);
Up = Vector3D.Multiply(Up, rotationMatrix);
UpdateSettings();
}
/// <summary>
/// Create a transform matrix that translates from skeleton space
/// into a space whose origin is the center of the screen and has the
/// same units as skeleton space.
/// </summary>
/// <param name="sensorOffset">vector, in meters, from the center of the display to the center of the Kinect sensor</param>
/// <param name="sensorElevationAngle">elevation angle of the sensor in degrees</param>
/// <returns>transform matrix</returns>
public static Matrix3D SensorToScreenPositionTransform(Vector3D sensorOffset, double sensorElevationAngle)
{
var rotateIntoSensorSpace = new Matrix3D();
rotateIntoSensorSpace.Rotate(new Quaternion(new Vector3D(1.0, 0.0, 0.0), sensorElevationAngle));
var eye = -sensorOffset;
eye = rotateIntoSensorSpace.Transform(eye);
var normalFromEye = new Vector3D(0.0, 0.0, 1.0);
normalFromEye = rotateIntoSensorSpace.Transform(normalFromEye);
var up = new Vector3D(0.0, 1.0, 0.0);
return LookAt(eye, normalFromEye, up);
}
/// <summary>
/// 回転行列を取得。
/// </summary>
/// <returns></returns>
public Matrix3D RotationMatrix()
{
var mat = new Matrix3D();
mat.Rotate(mRotationCurrent);
return mat;
}
/// <summary>
/// This overload will rotate arrays of different types. Just pass null if you don't have any of that type
/// </summary>
public static void GetRotatedVector(this Quaternion quaternion, Vector3D[] vectors, Point3D[] points, DoubleVector[] doubleVectors)
{
Matrix3D matrix = new Matrix3D();
matrix.Rotate(quaternion);
MatrixTransform3D transform = new MatrixTransform3D(matrix);
if (vectors != null)
{
transform.Transform(vectors);
}
if (points != null)
{
transform.Transform(points);
}
if (doubleVectors != null)
{
for (int cntr = 0; cntr < doubleVectors.Length; cntr++)
{
doubleVectors[cntr] = new DoubleVector(transform.Transform(doubleVectors[cntr].Standard), transform.Transform(doubleVectors[cntr].Orth));
}
}
}
public static void GetRotatedVector(this Quaternion quaternion, DoubleVector[] doubleVectors)
{
Matrix3D matrix = new Matrix3D();
matrix.Rotate(quaternion);
MatrixTransform3D transform = new MatrixTransform3D(matrix);
for (int cntr = 0; cntr < doubleVectors.Length; cntr++)
{
doubleVectors[cntr] = new DoubleVector(transform.Transform(doubleVectors[cntr].Standard), transform.Transform(doubleVectors[cntr].Orth));
}
}
private void RotateCameraSpherical(double dx, double dy)
{
// Figure out how to rotate the screen into world coords
Quaternion quatScreenToWorld = GetRotationForDelta();
// Turn the mouse movement into 3D rotation
Quaternion quat = GetSphericalMovement(dx, dy);
// Rotate that
quat = new Quaternion(quatScreenToWorld.GetRotatedVector(quat.Axis), quat.Angle * .1d); // also, make it less twitchy
//quat = new Quaternion(quat.Axis, quat.Angle * .1d);
// Create a matrix that will perform the rotation
Matrix3D matrix = new Matrix3D();
matrix.Rotate(quat);
// Store the camera's stats
Vector3D[] vectors = new Vector3D[2];
vectors[0] = _camera1.LookDirection;
vectors[1] = _camera1.UpDirection;
// Rotate those stats
matrix.Transform(vectors);
// Apply the new stats
_camera1.LookDirection = vectors[0];
_camera1.UpDirection = vectors[1];
}
public static DoubleVector GetRotatedVector(this Quaternion quaternion, DoubleVector doubleVector)
{
Matrix3D matrix = new Matrix3D();
matrix.Rotate(quaternion);
MatrixTransform3D transform = new MatrixTransform3D(matrix);
return new DoubleVector(transform.Transform(doubleVector.Standard), transform.Transform(doubleVector.Orth));
}
private void RotateCameraAroundLookDir(double curX, double curY, double dx, double dy)
{
const double ROTATESPEED = .005d;
// Calculate angle
//double radians = ROTATESPEED * Math.Sqrt(Math.Pow(dx, 2) + Math.Pow(dy, 2));
double radians = ROTATESPEED * Math.Sqrt((dx * dx) + (dy * dy));
double degrees = Math1D.RadiansToDegrees(radians);
if (radians < 0)
{
MessageBox.Show("ya");
}
// See if I should negate the angle
//NOTE: This logic is flawed. I fixed this later by taking curXY cross prevXY
if (curX >= 0 && curY >= 0)
{
// Q1
if (dx > 0 || dy < 0)
{
degrees *= -1;
}
}
else if (curX <= 0 && curY >= 0)
{
// Q2
if (dx > 0 || dy > 0)
{
degrees *= -1;
}
}
else if (curX <= 0 && curY <= 0)
{
// Q3
if (dx < 0 || dy > 0)
{
degrees *= -1;
}
}
else if (curX >= 0 && curY <= 0)
{
// Q4
if (dx < 0 || dy < 0)
{
degrees *= -1;
}
}
// Create a matrix that will perform the rotation
Matrix3D matrix = new Matrix3D();
matrix.Rotate(new Quaternion(_camera1.LookDirection, degrees));
// Rotate the camera
_camera1.UpDirection = matrix.Transform(_camera1.UpDirection);
}
private void CaptureMouseMove(object sender, MouseEventArgs e)
{
Point currentMousePoint = e.GetPosition(this);
if (this.pdbViewer.ActionType == PdbActionType.Rotate)
{
Vector3D currentMouseVector3D = this.ProjectToTrackball(currentMousePoint);
Vector3D axis = Vector3D.CrossProduct(
this.previousMouseVector, currentMouseVector3D);
double angle = 2 * Vector3D.AngleBetween(
this.previousMouseVector, currentMouseVector3D);
if (axis.LengthSquared > 0)
{
QuaternionRotation3D rotation =
this.rotateTransform.Rotation as QuaternionRotation3D;
if (rotation != null)
{
rotation.Quaternion = new Quaternion(axis, angle) *
rotation.Quaternion;
}
}
this.previousMouseVector = currentMouseVector3D;
}
else if (this.pdbViewer.ActionType == PdbActionType.Select ||
this.pdbViewer.ActionType == PdbActionType.Deselect &&
currentMousePoint != this.previousMousePoint)
{
double left = Math.Min(this.previousMousePoint.X, currentMousePoint.X);
double top = Math.Min(this.previousMousePoint.Y, currentMousePoint.Y);
double right = this.ActualWidth -
Math.Max(this.previousMousePoint.X, currentMousePoint.X);
double bottom = this.ActualHeight -
Math.Max(this.previousMousePoint.Y, currentMousePoint.Y);
left = Math.Max(0, left);
top = Math.Max(this.CaptureElement.TranslatePoint(new Point(), this).Y, top);
right = Math.Max(0, right);
bottom = Math.Max(0, bottom);
this.selectionRectangle.Margin = new Thickness(left, top, right, bottom);
this.selectionRectangle.Visibility = Visibility.Visible;
Rect selectionRect = new Rect(this.previousMousePoint, currentMousePoint);
foreach (Atom atom in this.Molecule.Atoms)
{
Point? atomPoint = this.GetViewportCoordinatePoint3Ds(atom);
bool contained = atomPoint != null && selectionRect.Contains(atomPoint.Value);
if (contained)
{
if (this.pdbViewer.ActionType == PdbActionType.Select)
atom.ShowAsSelected = true;
else if (this.pdbViewer.ActionType == PdbActionType.Deselect)
atom.ShowAsSelected = false;
}
else
{
atom.ShowAsSelected = atom.IsSelected;
}
}
}
else if (this.pdbViewer.ActionType == PdbActionType.Translate)
{
double multiplier = 2 * Math.Tan(Math.PI / 8) * cameraOffset /
this.scaleTransform.ScaleX;
double deltaX = (currentMousePoint.X - this.previousMousePoint.X) /
this.viewport.ActualWidth * multiplier;
double deltaY = -(currentMousePoint.Y - this.previousMousePoint.Y) /
this.viewport.ActualHeight * multiplier;
Vector3D deltaVector = new Vector3D(deltaX, deltaY, 0);
QuaternionRotation3D rotation =
(QuaternionRotation3D)this.rotateTransform.Rotation;
Matrix3D matrix = new Matrix3D();
matrix.Rotate(rotation.Quaternion);
matrix.Invert();
deltaVector = matrix.Transform(deltaVector);
this.translateTransform.OffsetX += deltaVector.X;
this.translateTransform.OffsetY += deltaVector.Y;
this.translateTransform.OffsetZ += deltaVector.Z;
this.previousMousePoint = currentMousePoint;
}
}
/// <summary>
/// Rotate camera using 'Turntable' rotation.
/// </summary>
/// <param name="delta">
/// The relative change in position.
/// </param>
/// <param name="rotateAround">
/// The point to rotate around.
/// </param>
public void RotateTurntable(Vector delta, Point3D rotateAround)
{
Vector3D relativeTarget = rotateAround - this.CameraTarget;
Vector3D relativePosition = rotateAround - this.CameraPosition;
Vector3D up = this.ModelUpDirection;
Vector3D dir = this.CameraLookDirection;
dir.Normalize();
Vector3D right = Vector3D.CrossProduct(dir, this.CameraUpDirection);
right.Normalize();
double d = -0.5;
if (this.CameraMode != CameraMode.Inspect)
{
d *= -0.2;
}
d *= this.RotationSensitivity;
var q1 = new Quaternion(up, d * delta.X);
var q2 = new Quaternion(right, d * delta.Y);
Quaternion q = q1 * q2;
var m = new Matrix3D();
m.Rotate(q);
Vector3D newUpDirection = m.Transform(this.CameraUpDirection);
Vector3D newRelativeTarget = m.Transform(relativeTarget);
Vector3D newRelativePosition = m.Transform(relativePosition);
Point3D newTarget = rotateAround - newRelativeTarget;
Point3D newPosition = rotateAround - newRelativePosition;
this.CameraLookDirection = newTarget - newPosition;
if (CameraMode == CameraMode.Inspect)
{
this.CameraPosition = newPosition;
}
this.CameraUpDirection = newUpDirection;
}
private void ApplyKeyBindings(double deltaT)
{
int movementPerpendicular = 0;
int movementForward = 0;
int movementUp = 0;
double speedModifier = 1;
foreach (InputBindings.KeyBinding kb in WorldViewModel.InputBindings.KeyBindings
.Where(kb => kb.KeyCombo.All(k => _keyPressed(k))))
{
UnFollow();
if (kb.Action == InputBindings.KeyAction.Up)
movementUp++;
else if (kb.Action == InputBindings.KeyAction.Down)
movementUp--;
else if (kb.Action == InputBindings.KeyAction.Left)
movementPerpendicular++;
else if (kb.Action == InputBindings.KeyAction.Right)
movementPerpendicular--;
else if (kb.Action == InputBindings.KeyAction.TurnLeft)
Heading += deltaT * 200;
else if (kb.Action == InputBindings.KeyAction.TurnRight)
Heading -= deltaT * 200;
else if (kb.Action == InputBindings.KeyAction.TiltUp)
Tilt += deltaT * (AngleRangeHigh - AngleRangeLow) / 2.0;
else if (kb.Action == InputBindings.KeyAction.TiltDown)
Tilt -= deltaT * (AngleRangeHigh - AngleRangeLow) / 2.0;
else if (kb.Action == InputBindings.KeyAction.Walk)
speedModifier = 0.2;
else if (kb.Action == InputBindings.KeyAction.Forward)
movementForward++;
else if (kb.Action == InputBindings.KeyAction.Back)
movementForward--;
else if (kb.Action == InputBindings.KeyAction.Home)
Home.Execute(null);
else if (kb.Action == InputBindings.KeyAction.FollowSelected)
FollowSelected.Execute(null);
else if (kb.Action == InputBindings.KeyAction.Possess)
PossessEntity.Execute(null);
else if (kb.Action == InputBindings.KeyAction.Create)
_actionState = InputBindings.ActionState.CreateAction;
else
throw new Exception("Unexpected keyboard binding " + kb.Action);
}
// Set Position and Rotation
Rotation = new Quaternion(ZAxis, Heading) * new Quaternion(YAxis, -Tilt);
if (movementPerpendicular == 0 && movementForward == 0 && movementUp == 0)
{
MobileState = EnumMobileState.Standing;
return;
}
if (speedModifier < 1)
MobileState = EnumMobileState.Walking;
else
MobileState = EnumMobileState.Running;
var transformHeading = new Matrix3D();
transformHeading.Rotate(new Quaternion(ZAxis, Heading));
var changeInPosition = new Vector3D(
movementForward * _landSpeed,
movementPerpendicular * _landSpeed,
movementUp * (DistanceRangeHigh - DistanceRangeLow) / 10.0);
Position += changeInPosition * transformHeading * deltaT * speedModifier;
if (Position.Z < DistanceRangeLow)
Position.Z = DistanceRangeLow;
else if (Position.Z > DistanceRangeHigh)
Position.Z = DistanceRangeHigh;
}
void WorkShop_MouseMove(object sender, MouseEventArgs e)
{
if (ActionType == ActionType.Rotate)
{
var currentMousePoint = e.GetPosition(this);
var currentMouseVector = ProjectToTrackBall(currentMousePoint);
Vector3D axis = Vector3D.CrossProduct(PreviousMouseVector, currentMouseVector);
double angle = Vector3D.AngleBetween(PreviousMouseVector, currentMouseVector);
if (axis.LengthSquared > 0)
{
var rotation = RotateTransform.Rotation as QuaternionRotation3D;
if (rotation != null)
{
rotation.Quaternion = new Quaternion(axis, angle) * rotation.Quaternion;
PreviousMouseVector = currentMouseVector;
}
}
}
else if (ActionType == ActionType.Translate)
{
var currentMousePoint = e.GetPosition(this);
double multiplier = 2 * (Math.Tan(Math.PI / 8) / ScaleTransform.ScaleX) * 4;
double deltaX = ((currentMousePoint.X - PreviousMousePoint.X) / Space.ActualWidth) * multiplier;
double deltaY = -((currentMousePoint.Y - PreviousMousePoint.Y) / Space.ActualHeight) * multiplier;
var deltaVector = new Vector3D(deltaX, deltaY, 0);
var rotation = (QuaternionRotation3D)RotateTransform.Rotation;
var matrix = new Matrix3D();
matrix.Rotate(rotation.Quaternion);
matrix.Invert();
deltaVector = matrix.Transform(deltaVector);
TranslateTransform.OffsetX += deltaVector.X;
TranslateTransform.OffsetY += deltaVector.Y;
TranslateTransform.OffsetZ += deltaVector.Z;
PreviousMousePoint = currentMousePoint;
}
}
private Matrix3D SetTransformMatrix(double offsetX, double offsetY, double offsetZ, int rotateAroundX)
{
Matrix3D resultMatrix = new Matrix3D();
resultMatrix.Rotate(new Quaternion(new Vector3D(10, 0, 0), -rotateAroundX));
resultMatrix.Translate(new Vector3D(offsetX, offsetY, -offsetZ));
return resultMatrix;
}
/// <summary>
/// rotate model by angle
/// </summary>
/// <param name="axis">axis for rotate</param>
/// <param name="rotateAngle">rotation angle</param>
public void RotateObjectByAngle(Vector3D axis, double rotateAngle)
{
try
{
if (_CurrentModel != null)
{
Transform3DGroup group = _CurrentModel.Content.Transform as Transform3DGroup;
if (group.Children.Count > 0)
{
for (int i = 0; i < group.Children.Count; i++)
{
MatrixTransform3D rtrans = group.Children[i] as MatrixTransform3D;
if (rtrans != null)
{
Matrix3D matrix = rtrans.Matrix;
matrix.Rotate(new Quaternion(axis, rotateAngle));
//get current radius of X axis
double rotaterad = Math.Acos(matrix.M22);
if ((rotaterad * (180 / Math.PI)) > _MaxRotateXDegree)
{
matrix.Rotate(new Quaternion(axis, -rotateAngle));
}
MatrixTransform3D mat = new MatrixTransform3D(matrix);
rtrans = mat;
group.Children[i] = rtrans;
}
}
}
else
{
Matrix3D matrix = new Matrix3D();
matrix.Rotate(new Quaternion(axis, rotateAngle));
MatrixTransform3D mat = new MatrixTransform3D(matrix);
group.Children.Add(mat);
}
}
}
catch (Exception ex)
{
throw ex;
}
}
/// <summary>
/// Gets a transform matrix representing the transform of the robot model
/// </summary>
/// <returns></returns>
public Matrix3D GetTransformMatrix()
{
Matrix3D mat = new Matrix3D();
Vector3D offset = new Vector3D(-OriginX, -OriginY, -OriginZ);
mat.Translate(offset);
Vector3D xAxis = new Vector3D(1, 0, 0);
Quaternion roll = new Quaternion(xAxis, OriginR * 180 / Math.PI);
Vector3D yAxis = new Vector3D(0, 1, 0);
Quaternion pitch = new Quaternion(yAxis, OriginP * 180 / Math.PI);
Vector3D zAxis = new Vector3D(0, 0, 1);
Quaternion yaw = new Quaternion(zAxis, OriginW * 180 / Math.PI);
mat.Rotate(roll);
mat.Rotate(pitch);
mat.Rotate(yaw);
RobotInfo.WriteToLogFile("Transform Matrix Created");
return mat;
}
/// <summary>
/// Rotate around three axes.
/// </summary>
/// <param name="p1">
/// The previous mouse position.
/// </param>
/// <param name="p2">
/// The current mouse position.
/// </param>
/// <param name="rotateAround">
/// The point to rotate around.
/// </param>
public void RotateTurnball(Point p1, Point p2, Point3D rotateAround)
{
this.InitTurnballRotationAxes(p1);
Vector delta = p2 - p1;
Vector3D relativeTarget = rotateAround - this.CameraTarget;
Vector3D relativePosition = rotateAround - this.CameraPosition;
double d = -1;
if (this.CameraMode != CameraMode.Inspect)
{
d = 0.2;
}
d *= this.RotationSensitivity;
var q1 = new Quaternion(this.rotationAxisX, d * delta.X);
var q2 = new Quaternion(this.rotationAxisY, d * delta.Y);
Quaternion q = q1 * q2;
var m = new Matrix3D();
m.Rotate(q);
Vector3D newLookDir = m.Transform(this.CameraLookDirection);
Vector3D newUpDirection = m.Transform(this.CameraUpDirection);
Vector3D newRelativeTarget = m.Transform(relativeTarget);
Vector3D newRelativePosition = m.Transform(relativePosition);
Vector3D newRightVector = Vector3D.CrossProduct(newLookDir, newUpDirection);
newRightVector.Normalize();
Vector3D modUpDir = Vector3D.CrossProduct(newRightVector, newLookDir);
modUpDir.Normalize();
if ((newUpDirection - modUpDir).Length > 1e-8)
{
newUpDirection = modUpDir;
}
Point3D newTarget = rotateAround - newRelativeTarget;
Point3D newPosition = rotateAround - newRelativePosition;
Vector3D newLookDirection = newTarget - newPosition;
this.CameraLookDirection = newLookDirection;
if (CameraMode == CameraMode.Inspect)
{
this.CameraPosition = newPosition;
}
this.CameraUpDirection = newUpDirection;
}
/// <summary>
/// パーティクルの描画設定をまとめて行います。
/// </summary>
public override void RenderParticles(IEnumerable<Particle> particles)
{
base.RenderParticles(particles);
foreach (var particle in particles)
{
if (particle.Brush != null)
{
var opacity = (double)((particle.Color >> 24) & 0xFF) / 255.0;
particle.Brush.Opacity = EffectObject.InheritedOpacity * opacity;
}
if (particle.Material != null)
{
var em = particle.Material as EmissiveMaterial;
if (em != null)
{
em.Color = ToColor(particle.Color);
}
else
{
var dm = particle.Material as DiffuseMaterial;
if (dm != null)
{
dm.Color = ToColor(particle.Color);
}
}
}
// 行列変換
var m = new Matrix3D();
if (particle.Scale != 1.0)
{
m.Scale(new Vector3D(particle.Scale, particle.Scale, 1.0));
}
if (particle.Rotation != 0.0)
{
double rot = MathEx.ToDeg(particle.Rotation);
m.Rotate(new Quaternion(new Vector3D(0, 0, 1), rot));
}
m.Translate(new Vector3D(particle.X, particle.Y, -15.0));
particle.Model.Transform = new MatrixTransform3D(m);
}
}
public void RotateTwoAxes(double dx, double dy)
{
Vector3D up = ModelUpDirection; // new Vector3D(0, 0, 1);
Vector3D dir = LookDirection;
dir.Normalize();
Vector3D right = Vector3D.CrossProduct(dir, UpDirection);
right.Normalize();
double d = -0.5;
if (CameraMode == CameraMode.WalkAround)
d = 0.1;
d *= RotationSensitivity;
var q1 = new Quaternion(up, d*dx);
var q2 = new Quaternion(right, d*dy);
Quaternion q = q1*q2;
var m = new Matrix3D();
m.Rotate(q);
Vector3D newLookDir = m.Transform(LookDirection);
Vector3D newUpDir = m.Transform(UpDirection);
right = Vector3D.CrossProduct(newLookDir, newUpDir);
right.Normalize();
Vector3D modUpDir = Vector3D.CrossProduct(right, newLookDir);
modUpDir.Normalize();
if ((newUpDir - modUpDir).Length > 1e-8)
newUpDir = modUpDir;
LookDirection = newLookDir;
UpDirection = newUpDir;
}
/// <summary>
/// Determines whether the back of the plane is showing, given a rotation.
/// Credit to Joel Pryde.
/// </summary>
/// <param name="x">The x rotation.</param>
/// <param name="y">The y rotation.</param>
/// <param name="z">The z rotation.</param>
/// <returns>
/// <c>true</c> if the back of the plane is showing, given a rotation; otherwise, <c>false</c>.
/// </returns>
private static bool IsBackShowingRotation(double x, double y, double z)
{
Matrix3D rotMatrix = new Matrix3D();
rotMatrix.Rotate(new Quaternion(new Vector3D(1, 0, 0), x));
rotMatrix.Rotate(new Quaternion(new Vector3D(0, 1, 0) * rotMatrix, y));
rotMatrix.Rotate(new Quaternion(new Vector3D(0, 0, 1) * rotMatrix, z));
Vector3D transformZ = rotMatrix.Transform(new Vector3D(0, 0, 1));
return Vector3D.DotProduct(new Vector3D(0, 0, 1), transformZ) < 0;
}
/// <summary>
/// Rotates the vector around the angle in degrees
/// </summary>
public static Vector3D GetRotatedVector(this Vector3D vector, Vector3D axis, double angle)
{
Matrix3D matrix = new Matrix3D();
matrix.Rotate(new Quaternion(axis, angle));
MatrixTransform3D transform = new MatrixTransform3D(matrix);
return transform.Transform(vector);
}
Matrix3D CalculateRotationMatrix(double x, double y, double z)
{
Matrix3D matrix = new Matrix3D();
matrix.Rotate(new Quaternion(new Vector3D(1, 0, 0), x));
matrix.Rotate(new Quaternion(new Vector3D(0, 1, 0) * matrix, y));
matrix.Rotate(new Quaternion(new Vector3D(0, 0, 1) * matrix, z));
return matrix;
}
// I copy the code in each of these overloads, rather than make a private method to increase speed
//TODO: I don't use these rotate extension methods anymore, but it would be worth testing whether to use this matrix transform, or use: new RotateTransform3D(new QuaternionRotation3D(quaternion))
public static Vector3D GetRotatedVector(this Quaternion quaternion, Vector3D vector)
{
Matrix3D matrix = new Matrix3D();
matrix.Rotate(quaternion);
MatrixTransform3D transform = new MatrixTransform3D(matrix);
return transform.Transform(vector);
}
public static Point3D GetRotatedVector(this Quaternion quaternion, Point3D point)
{
Matrix3D matrix = new Matrix3D();
matrix.Rotate(quaternion);
MatrixTransform3D transform = new MatrixTransform3D(matrix);
return transform.Transform(point);
}
protected virtual void NewTransform()
{
if (lockCount != 0)
return;
Matrix3D m = new Matrix3D();
m.Scale(new Vector3D(ScaleX, ScaleY, ScaleZ));
m.Rotate(Rotation1);
m.Rotate(Rotation2);
m.Translate(new Vector3D(Position.X, Position.Y, Position.Z));
m.Rotate(Rotation3);
Transform = new MatrixTransform3D(m);
}
public void UpdateOrCreateJoint(Joint joint)
{
int segments = 5;
double jointRadius = 0.07;
double boneRadius = 0.03;
int jointId = (int)joint.ID;
int connectedToJoint = SkeletonMetadata.BoneConnectionMapping[jointId];
_jointPositions[jointId] = new Vector3D(joint.Position.X, joint.Position.Y, joint.Position.Z);
// Create new 3d cube for the joint if not yet existing
GeometryModel3D model;
if (_joints[jointId] != null)
{
model = _joints[jointId];
}
else
{
model = Model3DFactory.CreateNormalizedSphere(defaultMaterial, segments);
_environment.Children.Add(model);
_joints[jointId] = model;
if (connectedToJoint >= 0)
{
GeometryModel3D cylinder = Model3DFactory.CreateNormalizedCylinder(defaultMaterial, segments);
_environment.Children.Add(cylinder);
_bones[jointId] = cylinder;
}
}
// Performance improvement: not using a transformation group, but multiply
// matrices first and use a single MatrixTransform3D
var matrix = new Matrix3D();
matrix.Scale(new Vector3D(jointRadius, jointRadius, jointRadius));
matrix.Translate(new Vector3D(joint.Position.X, joint.Position.Y, joint.Position.Z));
// Update position and the material/color (based on current tracking state), color right shoulder blue
if (joint.ID == JointID.ShoulderRight)
model.Material = rightShoulderIndicatorMaterial;
else
model.Material = _trackingStateMaterials[joint.TrackingState];
model.Transform = new MatrixTransform3D(matrix);
if (connectedToJoint >= 0)
{
GeometryModel3D bone = _bones[jointId];
Vector3D boneStart = _jointPositions[jointId];
Vector3D boneEnd = _jointPositions[connectedToJoint];
Vector3D boneCenter = (boneStart + boneEnd) / 2;
Vector3D boneVector = boneEnd - boneStart;
// Again, compute a single transformation matrix and apply it to each bone
var boneMatrix = new Matrix3D();
boneMatrix.Scale(new Vector3D(boneRadius, boneRadius, boneVector.Length));
boneMatrix.Rotate(GetQuaternionFromVectors(new Vector3D(0, 0, 1), boneVector));
boneMatrix.Translate(boneCenter);
bone.Material = _trackingStateMaterials[joint.TrackingState];
bone.Transform = new MatrixTransform3D(boneMatrix); ;
}
}