public static void OrbitYZ(this Camera camera, double alpha)
{
var offset = camera.Frame.Offset;
var ex = camera.Frame.Ex;
var ey = camera.Frame.Ey;
var ez = camera.Frame.Ez;
var target = offset + ey * camera.Distance;
var back = Matrix44D.CreateCoordinateSystem(target, ex, ey, ez);
var trans = back.Inverse();
ey = new Vector3D(0.0, 1.0, 0.0);
ez = new Vector3D(0.0, 0.0, 1.0);
offset = trans * offset;
var rotateYZ = Matrix44D.CreateRotation(new Vector3D(1, 0, 0), alpha);
ey = rotateYZ * ey;
ez = rotateYZ * ez;
offset = rotateYZ * offset;
offset = back * offset;
ey = back * ey;
ez = back * ez;
ey.Normalize();
ez.Normalize();
var newFrame = Matrix44D.CreateCoordinateSystem(offset, ex, ez);
camera.Frame = newFrame;
}
public void RotationMatrix44()
{
double angle = -1.6;
Vector3D axis = new Vector3D(1.0, 2.0, -3.0);
QuaternionD q = QuaternionD.CreateRotation(axis, angle);
Matrix44D m44 = Matrix44D.CreateRotation(axis, angle);
Assert.IsTrue(Matrix44D.AreNumericallyEqual(q.ToRotationMatrix44(), m44));
}
public void CreateRotationTest_WhenParameterOffsetAxisAndAngle_ThenPositionIsRotatedAndTranslated()
{
var axis = new Vector3D(1, 0, 0);
var rot = Matrix44D.CreateRotation(new Position3D(0, 0, 10), axis, 90.0.DegToRad());
var pos = rot * new Position3D(0.0, 0.0, 0.0);
Assert.Equal(new Position3D(0.0, 10.0, 10.0), pos);
}
public void CreateRotationTest_WhenParameterOnlyAxisAndAngle_ThenPositioIsOnlyRotated()
{
var axis = new Vector3D(1, 0, 0);
var rot = Matrix44D.CreateRotation(axis, 90.0.DegToRad());
Assert.Equal(new Position3D(), rot.Offset);
Assert.Equal(new Vector3D(), rot.Translation);
var pos = rot * new Position3D(0.0, 0.0, 1.0);
Assert.Equal(new Position3D(0.0, -1.0, 0.0), pos);
}
public static Matrix44D ToMatrix44D(this CardanFrame eulerFrame)
{
var alphaRotation = Matrix44D.CreateRotation(new Vector3D(1.0, 0.0, 0.0), eulerFrame.AlphaAngleAxisX);
var betaRotation = Matrix44D.CreateRotation(new Vector3D(0.0, 1.0, 0.0), eulerFrame.BetaAngleAxisY);
var gammaRotation = Matrix44D.CreateRotation(new Vector3D(0.0, 0.0, 1.0), eulerFrame.GammaAngleAxisZ);
var translation = Matrix44D.CreateTranslation(eulerFrame.Translation);
var matrix = translation * gammaRotation * betaRotation * alphaRotation;
return(matrix);
}
public Matrix44D CreateFrame()
{
var rotAlpha = Matrix44D.CreateRotation(new Vector3D(1, 0, 0), CreateAngle());
var rotBeta = Matrix44D.CreateRotation(new Vector3D(0, 1, 0), CreateAngle());
var rotGamma = Matrix44D.CreateRotation(new Vector3D(0, 0, 1), CreateAngle());
var translation = Matrix44D.CreateTranslation(CreateVector());
var randomMatrix = translation * rotAlpha * rotBeta * rotGamma;
return(randomMatrix);
}
public static Scene CreateAndPopulateScene()
{
var scene = new Scene();
var cube = Cube.Create(10.0);
cube.Frame = Matrix44D.CreateTranslation(new Vector3D(0, 0, 5));
scene.Bodies.Add(cube);
var cube2 = Cube.Create(10.0);
cube2.Frame = Matrix44D.CreateTranslation(new Vector3D(0, 0, 15)) * Matrix44D.CreateRotation(new Vector3D(0, 0, 1), 45.0.DegToRad());;
cube2.Sensor = new CylinderSensor(new Vector3D(0, 0, 1));
scene.Bodies.Add(cube2);
var cylinder = Cylinder.Create(16, 4.0, 10.0);
cylinder.Frame = Matrix44D.CreateTranslation(new Vector3D(10, 10, 5.1));
cylinder.Sensor = new LinearSensor(new Vector3D(0, 1, 0));
scene.Bodies.Add(cylinder);
var floor = Floor.Create(4, 20);
scene.Bodies.Add(floor);
double[][] segments = new double[][]
{
new [] { 0.0, 0.0, 5.0, 0.0 },
new [] { 5.0, 0.0, 5.0, 1.0 },
new [] { 5.0, 1.0, 1.0, 5.0 },
new [] { 1.0, 5.0, 1.0, 7.0 },
new [] { 1.0, 7.0, 5.0, 11.0 },
new [] { 5.0, 11.0, 5.0, 12.0 },
new [] { 5.0, 12.0, 0.0, 12.0 },
};
bool[] borderFlags = new bool[] { true, true, true, true, true, true, true };
bool[] facetsFlags = new bool[] { false, false, false, false, false, false, false };
var rotationBody = RotationBody.Create(16, segments, borderFlags, facetsFlags);
rotationBody.Sensor = new PlaneSensor(new Vector3D(0, 0, 1));
rotationBody.Frame = Matrix44D.CreateTranslation(new Vector3D(30, -30, 0.1));
scene.Bodies.Add(rotationBody);
var sphere = Sphere.Create(16, 8);
sphere.Sensor = new SphereSensor();
sphere.Frame = Matrix44D.CreateTranslation(new Vector3D(-30, 30, 10));
scene.Bodies.Add(sphere);
return(scene);
}
public void OperatorMultiplyTest_WhenTranslationWithRoationAreMultiplidWithPosition_ThenPositionIsTranslatedAndThenRotated()
{
var first = Matrix44D.CreateRotation(new Position3D(), new Vector3D(0, 0, 1), 90.0.DegToRad());
var second = Matrix44D.CreateTranslation(new Vector3D(100.0, 0.0, 0.0));
var position = new Position3D(1.0, 0.0, 0.0);
var newPosition = first * second * position;
var expected = new Position3D(0.0, 101.0, 0.0);
Assert.Equal(expected, newPosition);
}
public static void SetCamera(this Camera camera, Position3D target, double alpha, double beta, double distance)
{
alpha = alpha.DegToRad();
beta = beta.DegToRad();
var rotAlpha = Matrix44D.CreateRotation(new Position3D(), new Vector3D(0, 0, 1), alpha);
var rotBeta = Matrix44D.CreateRotation(new Position3D(), new Vector3D(1, 0, 0), -beta);
var translation = Matrix44D.CreateTranslation(new Vector3D(0.0, -distance, 0.0));
var targetTranslation = Matrix44D.CreateTranslation(camera.Target.ToVector3D());
var frame = targetTranslation * rotAlpha * rotBeta * translation;
camera.Frame = frame;
camera.Target = target;
}
private static void Process(
Body body,
Position3D bodyTouchPosition,
double startX,
double startY,
Axis3D endMoveRay,
Axis3D cylinderAxis,
double canvasWidth,
double canvasHeight,
Camera camera)
{
var angle = CalculateAngle(bodyTouchPosition, startX, startY, endMoveRay, cylinderAxis, canvasWidth, canvasHeight, camera);
var rotation = Matrix44D.CreateRotation(cylinderAxis.Offset, cylinderAxis.Direction, angle);
body.Frame = rotation * body.Frame;
}
private static double CalculateAngleSign(Axis3D cylinderAxis, Position3D start, Position3D end)
{
var axisFrame = Matrix44D.CreateRotation(cylinderAxis.Offset, cylinderAxis.Direction);
Matrix44D invframe = axisFrame.Inverse();
var startInFrame = invframe * start;
var endInFrame = invframe * end;
var sx = startInFrame.X;
var sy = startInFrame.Y;
var ex = endInFrame.X;
var ey = endInFrame.Y;
var sign = TriangleMath.IsCounterClockwise(sx, sy, ex, ey, 0.0, 0.0) ? 1.0 : -1.0;
return(sign);
}
public static CardanFrame ToCardanFrame(this Matrix44D matrix)
{
var ex = matrix.Ex;
var ey = matrix.Ey;
var gammaAngleAxisZ = (ex.X, ex.Y).ToAngle();
var rotationXY = Matrix44D.CreateRotation(new Vector3D(0, 0, 1), -gammaAngleAxisZ);
var ex1 = rotationXY * ex;
var betaAngleAxisY = -(ex1.X, ex1.Z).ToAngle();
var rotationXZ = Matrix44D.CreateRotation(new Vector3D(0, 1, 0), -betaAngleAxisY);
var ey1 = rotationXZ * rotationXY * ey;
var alphaAngleAxisX = (ey1.Y, ey1.Z).ToAngle();
return(new CardanFrame(matrix.Translation, alphaAngleAxisX, betaAngleAxisY, gammaAngleAxisZ));
}
private static void Rotate(Body body, Axis3D axis, double angle)
{
var rotation = Matrix44D.CreateRotation(body.Frame.Offset, axis.Direction, angle);
body.Frame = rotation * body.Frame;
}
