private void button12_Click(object sender, EventArgs e)
{
try
{
double distance = Math.Sqrt(Math.Pow((pipe1._center_point.X - pipe2._center_point.X), 2) +
Math.Pow((pipe1._center_point.Y - pipe2._center_point.Y), 2) +
Math.Pow((pipe1._center_point.Z - pipe2._center_point.Z), 2));
tbDistance.Text = distance.ToString("0.##");
//바뀐 좌표계 U, N, E에서의 파이프2의 중심좌표
Console.WriteLine();
Console.WriteLine("Distance between pipe1 and pipe2 (mm)");
Point3d cp = pipe2._center_point.Copy();
Vector4d pipe2_CenterPoint = new Vector4d(cp.X, cp.Y, cp.Z, 1);
Vector4d res_cp = inv_H.Mult(pipe2_CenterPoint);
Point3d res = new Point3d(res_cp.X, res_cp.Y, res_cp.Z);
string temp = string.Format(" {0,7:0.##} , {1,7:0.##} , {2,7:0.##} ", res_cp.X, res_cp.Y, res_cp.Z);
tb_pipe2_cp_by_pipe1.Text = temp;
Console.WriteLine("pipe2's center point (pipe1's view point)");
SHOW_N_S_W_E(res);
}
catch (Exception)
{
MessageBox.Show("두 개의 파이프에 대한 데이터가 부족합니다.");
}
}
int like_gluUnProject(double winx, double winy, double winz, Matrix4d modelMatrix, Matrix4d projMatrix, int[] viewport, ref double objx, ref double objy, ref double objz)
{
Matrix4d finalMatrix;
Vector4d _in;
Vector4d _out;
finalMatrix = Matrix4d.Mult(modelMatrix, projMatrix);
finalMatrix.Invert();
_in.X = winx;
_in.Y = viewport[3] - winy;
_in.Z = winz;
_in.W = 1.0f;
// Map x and y from window coordinates
_in.X = (_in.X - viewport[0]) / viewport[2];
_in.Y = (_in.Y - viewport[1]) / viewport[3];
// Map to range -1 to 1
_in.X = _in.X * 2 - 1;
_in.Y = _in.Y * 2 - 1;
_in.Z = _in.Z * 2 - 1;
//__gluMultMatrixVecd(finalMatrix, _in, _out);
// check if this works:
_out = Vector4d.Transform(_in, finalMatrix);
if (_out.W == 0.0)
{
return(0);
}
_out.X /= _out.W;
_out.Y /= _out.W;
_out.Z /= _out.W;
objx = _out.X;
objy = _out.Y;
objz = _out.Z;
return(1);
}
public virtual void Draw(Vector3d location, Quaterniond rotation)
{
Matrix4d trans;
Matrix4d rot;
Matrix4d sca;
Matrix4d temp;
Vector3d axis;
double angle;
// Translate and rotate stuff.
trans = Matrix4d.CreateTranslation(location);
rotation.ToAxisAngle(out axis, out angle);
Matrix4d.CreateFromAxisAngle(axis, angle, out rot);
sca = Matrix4d.Scale(scale.X, 1, scale.Y);
temp = Matrix4d.Mult(Graphics.Modelview, trans);
temp = Matrix4d.Mult(sca, Matrix4d.Mult(rot, temp));
GL.PushMatrix();
GL.LoadMatrix(ref temp);
//Console.WriteLine("Matrix'd");
// Do drawing stuff.
//GL.ClientActiveTexture(TextureUnit.Texture0);
//GL.ActiveTexture(TextureUnit.Texture1);
//GL.BindTexture(TextureTarget.Texture2D, tex);
//int loc = GL.GetUniformLocation(Graphics.CurrentShader, "tex1");
//GL.Uniform1(loc, 1);
//Console.WriteLine("Texture'd {0}", tex);
material.Draw();
geometry.Draw();
GL.PopMatrix();
}
public virtual void Draw(Vector3d location, Quaterniond rotation)
{
Matrix4d trans;
Matrix4d rot;
Matrix4d sca;
Matrix4d temp;
Vector3d axis;
double angle;
//Console.WriteLine("DDDDrawing at {0},{1},{2}", location.X, location.Y, location.Z);
// Translate and rotate stuff.
trans = Matrix4d.CreateTranslation(location);
rotation.ToAxisAngle(out axis, out angle);
Matrix4d.CreateFromAxisAngle(axis, angle, out rot);
sca = Matrix4d.Scale(scale.X, 1, scale.Y);
temp = Matrix4d.Mult(Graphics.Modelview, trans);
temp = Matrix4d.Mult(sca, Matrix4d.Mult(rot, temp));
//Console.WriteLine("MMMMatrix:\n{0}", temp);
GL.PushMatrix();
GL.LoadMatrix(ref temp);
//Console.WriteLine("Matrix'd");
// Do drawing stuff.
GL.ActiveTexture(TextureUnit.Texture0);
GL.BindTexture(TextureTarget.Texture2D, frames[animFrame]);
geometry.Draw();
GL.PopMatrix();
}
public static UMatrix4 product(UMatrix4 m1, UMatrix4 m2)
{
UMatrix4 r = default(UMatrix4);
r.Matrix = Matrix4d.Mult(m1.Matrix, m2.Matrix);
return(r);
}
public bool LocationToView(Location location, out PointF point)
{
// Move location to 3D earth
var pos3d = new Vector4d(location.Position, 1);
// Camera model
var m = Matrix4d.Mult(modelViewMatrix, projectionMatrix);
// Project into homogeneous 2D point
var pos2h = Vector4d.Transform(pos3d, m);
// Perform the perspective divide
var pos2 = pos2h / pos2h.W;
// Ignore points behind us
if (pos2h.W < 0)
{
point = PointF.Empty;
return(false);
}
// Console.WriteLine ("{0} = {1}", "W", pos2h.W);
// Stretch into our view
var fr = videoCameraView.Frame;
point = new PointF(
fr.X + (float)((pos2.X + 1) * 0.5) * fr.Width,
fr.Y + (float)((-pos2.Y + 1) * 0.5) * fr.Height
);
return(true);
}
public static bool UnProject(Vector4d winPos, Matrix4d modelMatrix, Matrix4d projMatrix, double[] viewport, ref Vector4d objPos)
{
Matrix4d p = Matrix4d.Mult(modelMatrix, projMatrix);
Matrix4d finalMatrix = Matrix4d.Invert(p);
Vector4d @in = winPos;
// Map x and y from window coordinates
@in.X = (@in.X - viewport[0]) / viewport[2];
@in.Y = (@in.Y - viewport[1]) / viewport[3];
// Map to range -1 to 1
@in.X = @in.X * 2.0 - 1.0;
@in.Y = @in.Y * 2.0 - 1.0;
@in.Z = @in.Z * 2.0 - 1.0;
@in.W = 1.0;
Vector4d @out = Vector4d.Transform(@in, finalMatrix);
if (@out.W == 0.0)
{
return(false);
}
@out.X /= @out.W;
@out.Y /= @out.W;
@out.Z /= @out.W;
objPos = @out;
return(true);
}
private void UpdateSenzorToCamera()
{
// - translate by (-0.5, -aspectRatio * 0.5)
// - scale by (width, height, 1)
// - translate by (shiftX, shiftY, distanceZ)
// - [tilt around X by tiltX, ...]
var matrix = Matrix4d.CreateTranslation(-0.5, AspectRatio * -0.5, 0);
matrix = Matrix4d.Mult(matrix, Matrix4d.Scale(Width, Width, 1));
matrix = Matrix4d.Mult(matrix, Matrix4d.CreateTranslation(Shift));
TiltMatrix = Matrix4d.Identity;
if (Tilt.X != 0)
{
TiltMatrix = Matrix4d.CreateRotationX(Tilt.X);
}
if (Tilt.Y != 0)
{
TiltMatrix = Matrix4d.Mult(TiltMatrix, Matrix4d.CreateRotationY(Tilt.Y));
}
if (Tilt.Z != 0)
{
TiltMatrix = Matrix4d.Mult(TiltMatrix, Matrix4d.CreateRotationZ(Tilt.Z));
}
matrix = Matrix4d.Mult(matrix, TiltMatrix);
SenzorToCamera = matrix;
}
public Matrix4d GetMatrix()
{
Matrix4d matrix = Matrix4d.CreateRotationY(Utils.DEGREES_TO_RADIANS(this.Rotation));
matrix = Matrix4d.Mult(matrix, Matrix4d.CreateTranslation(Translation.X, 0, Translation.Y));
matrix = Matrix4d.Mult(matrix, Matrix4d.Scale(Scale, 1f, Scale));
return(matrix);
}
public static Matrix4d Multiply(this Matrix4d left, Matrix4d right)
{
Matrix4d result;
Matrix4d.Mult(ref left, ref right, out result);
return(result);
}
/// <summary>
/// Projects a 2d screenspace coordinate to world coordinates.
/// </summary>
public static Vector3d Unproject(Vector3d Point, Matrix4d View, Matrix4d Proj)
{
Matrix4d ma = Matrix4d.Mult(View, Proj);
Matrix4d ima = Matrix4d.Invert(ma);
Vector4d coord = new Vector4d(Point.X, Point.Y, Point.Z, 1.0);
Vector4d res = Vector4d.Transform(coord, ima);
return(new Vector3d(res.X / res.W, res.Y / res.W, res.Z / res.W));
}
public virtual void Rotate(double y, double p, double r)
{
yaw = Matrix4dExtension.DegreesToRadians(y);
pitch = Matrix4dExtension.DegreesToRadians(p);
roll = Matrix4dExtension.DegreesToRadians(r);
Matrix4d rNew = yawPitchRoll(yaw, pitch, 0.0f);
R = Matrix4d.Mult(R, rNew);
Update();
}
public void Multiply()
{
var inputL = GetTestMatrix();
var inputR = GetTestMatrix();
var inputSimdL = (NMatrix4d)inputL;
var inputSimdR = (NMatrix4d)inputR;
var expected = Matrix4d.Mult(inputL, inputR);
var actual = NMatrix4d.Multiply(inputSimdL, inputSimdR);
Asserts.AreEqual(expected, actual, "multiply");
}
public void PrepareMatrixSegment(byte seg, int hnumber, int anumber)
{
Matrix4d testmtx = Matrix4d.Identity;
testmtx = Matrix4d.Mult(Matrix4d.CreateTranslation(GlobalTranslation), testmtx);
int ofs = 0;
WriteNextMtx(hnumber, 0, -1, testmtx, ref ofs);
Nanami.Segments[seg] = new NanamiRCP.Components.RAMSegment(Nanami, HierarchyMatrices[hnumber], string.Empty, false, ramadr: (uint)(Nanami.RDRAM.Length - HierarchyMatrices[hnumber].Length));
}
private void UpdateObjectToWorld()
{
// - translate by (-0.5, -aspectRatio * 0.5)
// - scale by (width, height, 1)
// - translate by origin
var matrix = Matrix4d.CreateTranslation(-0.5, AspectRatio * -0.5, 0);
matrix = Matrix4d.Mult(matrix, Matrix4d.Scale(Width, Width, 1));
matrix = Matrix4d.Mult(matrix, Matrix4d.CreateTranslation(Plane.Origin));
ObjectToWorld = matrix;
}
private static Vector3d UnProject(Vector3d screen, Matrix4d modelviewMatrix, Matrix4d projectionMatrix, int[] viewport)
{
Vector4d pos = Vector4d.Zero;
pos.X = ((screen.X - (double)viewport[0]) / (double)viewport[2]) * 2.0 - 1.0;
pos.Y = ((screen.Y - (double)viewport[1]) / (double)viewport[3]) * 2.0 - 1.0;
pos.Z = (2.0 * screen.Z) - 1.0;
pos.W = 1.0;
Vector4d pos2 = Vector4d.Transform(pos, Matrix4d.Invert(Matrix4d.Mult(modelviewMatrix, projectionMatrix)));
return(new Vector3d(pos2.X, pos2.Y, pos2.Z) / pos2.W);
}
public void Draw(Vector2d location, int xOffset, int yOffset)
{
//Console.WriteLine("Location: {0},{1} offsets {2},{3}", location.X, location.Y, xOffset, yOffset);
if (location.X + DrawWidth > MapWidth ||
location.Y + DrawHeight > MapHeight ||
location.X < 0 ||
location.Y < 0)
{
string message = String.Format("Tried to draw out of bounds at {0},{1}", location.X, location.Y);
throw new IndexOutOfRangeException(message);
}
Vector3d location2 = new Vector3d(location.X, location.Y, 0);
Matrix4d trans = Matrix4d.CreateTranslation(location2);
Matrix4d temp = Matrix4d.Mult(Graphics.Modelview, trans);
GL.PushMatrix();
GL.LoadMatrix(ref temp);
// Push current Array Buffer state so we can restore it later
GL.PushClientAttrib(ClientAttribMask.ClientVertexArrayBit);
GL.UseProgram(Graphics.Shaders[Graphics.TILESHADER]);
// Texture sheet
GL.ActiveTexture(TextureUnit.Texture0);
GL.BindTexture(TextureTarget.Texture2D, TileSheet);
GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)All.Nearest);
GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)All.Nearest);
SetupTexcoords(xOffset, yOffset, DrawWidth, DrawHeight);
// Set up buffers.
GL.BindBuffer(BufferTarget.ArrayBuffer, VboHandle);
GL.VertexPointer(3, VertexPointerType.Float, 0, (IntPtr)0);
GL.BindBuffer(BufferTarget.ArrayBuffer, TexCoordHandle);
GL.TexCoordPointer(2, TexCoordPointerType.Float, 0, (IntPtr)0);
GL.DrawArrays(BeginMode.Quads, 0, DrawWidth * DrawHeight * 4);
/*
* for(int i = 0; i < MapHeight; i++) {
* int vertsPerStrip = MapWidth * 4;
* int offset = i * vertsPerStrip;
* GL.DrawArrays(BeginMode.Quads, offset, vertsPerStrip);
* }
*/
// Restore the state
GL.UseProgram(Graphics.Shaders[Graphics.NORMALSHADER]);
GL.PopClientAttrib();
GL.PopMatrix();
}
public static void Rotate(this Matrix4d mat, double angle, double x, double y, double z)
{
if (angle == 0.0 || (x == 0.0 && y == 0.0 && z == 0))
{
return;
}
// convert to radians
angle = angle * MathBase.DegreesToRadians_Float;
// make a normalized quaternion
double w = Convert.ToSingle(Math.Cos(0.5f * angle));
double f = Convert.ToSingle(Math.Sin(0.5f * angle) / Math.Sqrt(x * x + y * y + z * z));
x *= f;
y *= f;
z *= f;
// convert the quaternion to a matrix
Matrix4d matrix = Matrix4d.Identity;
double ww = w * w;
double wx = w * x;
double wy = w * y;
double wz = w * z;
double xx = x * x;
double yy = y * y;
double zz = z * z;
double xy = x * y;
double xz = x * z;
double yz = y * z;
double s = ww - xx - yy - zz;
matrix[0, 0] = xx * 2 + s;
matrix[1, 0] = (xy + wz) * 2;
matrix[2, 0] = (xz - wy) * 2;
matrix[0, 1] = (xy - wz) * 2;
matrix[1, 1] = yy * 2 + s;
matrix[2, 1] = (yz + wx) * 2;
matrix[0, 2] = (xz + wy) * 2;
matrix[1, 2] = (yz - wx) * 2;
matrix[2, 2] = zz * 2 + s;
Matrix4d.Mult(ref mat, ref matrix, out mat);
}
public void Multiply_ByRef()
{
var inputL = GetTestMatrix();
var inputR = GetTestMatrix();
var inputSimdL = (NMatrix4d)inputL;
var inputSimdR = (NMatrix4d)inputR;
Matrix4d expected;
NMatrix4d actual;
Matrix4d.Mult(ref inputL, ref inputR, out expected);
NMatrix4d.Multiply(ref inputSimdL, ref inputSimdR, out actual);
Asserts.AreEqual(expected, actual, "multiply");
}
public static void Translate(this Matrix4d mat, double x, double y, double z)
{
if (x == 0.0 && y == 0.0 && z == 0)
{
return;
}
Matrix4d matrix = Matrix4d.Identity;
matrix[0, 3] = x;
matrix[1, 3] = y;
matrix[2, 3] = z;
Matrix4d.Mult(ref mat, ref matrix, out mat);
}
public static void Scale(this Matrix4d mat, double x, double y, double z)
{
if (x == 1 && y == 1 && z == 1)
{
return;
}
Matrix4d matrix = Matrix4d.Identity;
matrix[0, 0] = x;
matrix[1, 1] = y;
matrix[2, 2] = z;
Matrix4d.Mult(ref mat, ref matrix, out mat);
}
/// <summary>Calculate the Projection matrix - projects the 3d model space to the 2D screen</summary>
public void CalculateModelMatrix(Vector3d lookatd, Vector3d eyepositiond, double camerarotation)
{
LookAt = new Vector3((float)lookatd.X, (float)lookatd.Y, (float)lookatd.Z); // record for shader use
EyePosition = new Vector3((float)eyepositiond.X, (float)eyepositiond.Y, (float)eyepositiond.Z);
EyeDistance = (float)((lookatd - eyepositiond).Length);
// System.Diagnostics.Debug.WriteLine($"CMM {lookat} {eyeposition} dist {EyeDistance} {cameradirection} {camerarotation}");
Matrix4d mat;
if (InPerspectiveMode)
{
Vector3d camnormal = new Vector3d(0, ModelAxisPositiveZAwayFromViewer ? -1 : 1, 0);
if (camerarotation != 0) // if we have camera rotation, then rotate the up vector
{
Matrix4d rot = Matrix4d.CreateRotationZ((float)camerarotation.Radians());
camnormal = Vector3d.Transform(camnormal, rot);
}
mat = Matrix4d.LookAt(eyepositiond, lookatd, camnormal); // from eye, look at target, with normal giving the rotation of the look
}
else
{
Size scr = ViewPort.Size;
double orthoheight = (OrthographicDistance / 5.0f) * scr.Height / scr.Width; // this comes from the projection calculation, and allows us to work out the scale factor the eye vs lookat has
double scaler = orthoheight / EyeDistance;
// no create scale, do it manually.
Matrix4d scale = new Matrix4d(new Vector4d(scaler, 0, 0, 0), new Vector4d(0, scaler, 0, 0), new Vector4d(0, 0, scaler, 0), new Vector4d(0, 0, 0, 1));
mat = Matrix4d.CreateTranslation(-lookatd.X, 0, -lookatd.Z); // we offset by the negative of the position to give the central look
mat = Matrix4d.Mult(mat, scale); // translation world->View = scale + offset
Matrix4d rotcam = Matrix4d.CreateRotationX(90.0 * Math.PI / 180.0f); // flip 90 along the x axis to give the top down view
mat = Matrix4d.Mult(mat, rotcam);
}
// and turn it to float model matrix
ModelMatrix = new Matrix4((float)mat.M11, (float)mat.M12, (float)mat.M13, (float)mat.M14, (float)mat.M21, (float)mat.M22, (float)mat.M23, (float)mat.M24, (float)mat.M31, (float)mat.M32, (float)mat.M33, (float)mat.M34, (float)mat.M41, (float)mat.M42, (float)mat.M43, (float)mat.M44);
//System.Diagnostics.Debug.WriteLine("MM\r\n{0}", ModelMatrix);
ProjectionModelMatrix = Matrix4.Mult(ModelMatrix, ProjectionMatrix); // order order order ! so important.
CountMatrixCalcs++;
}
/// <summary>
/// sets the result point cloud - resp. pointCloudResultBest
/// </summary>
/// <param name="mypointCloudSource"></param>
/// <returns></returns>
private double SVD_Iteration(PointCloudVertices mypointCloudSource)
{
double bestResultMeanDistance = double.MaxValue;
double meanDistance = double.MaxValue;
pointCloudSourceCentered = CalculatePCA_Internal(mypointCloudSource);
Matrix4d myMatrixBestResult = Matrix4d.Identity;
// int i = -1;
//SVD_ForTwoPointCloudAlignment(-1, -1, ref bestResultMeanDistance, ref meanDistance, ref myMatrixBestResult, pointCloudSourceCentered.PCAAxes);
SVD_ForTwoPointCloudAlignment(-1, -1, ref bestResultMeanDistance, ref meanDistance, ref myMatrixBestResult, pointCloudSourceCentered.PCAAxes);
//leads to a lot of iterations
if (axesRotateEffect)
{
//additionally try other solutions: Invert all axes
if (meanDistance > thresholdConvergence)
{
for (int i = -1; i < 3; i++)
{
PointCloudVertices sourceAxes = InvertAxes(pointCloudSourceCentered, pointCloudSourceCentered.PCAAxes, i);
for (int j = -1; j < i; j++)
{
SVD_ForTwoPointCloudAlignment(i, j, ref bestResultMeanDistance, ref meanDistance, ref myMatrixBestResult, sourceAxes);
if (meanDistance < thresholdConvergence)
{
break;
}
}
if (meanDistance < thresholdConvergence)
{
break;
}
}
}
}
Matrix4d.Mult(ref myMatrixBestResult, ref this.Matrix, out this.Matrix);
//pointCloudResultBest
return(bestResultMeanDistance);
}
/// <summary>
/// Converts a screen space point into a corresponding point in world space.
/// </summary>
/// <param name="coordinate">The coordinate to project</param>
/// <param name="viewport">The viewport dimensions</param>
/// <param name="projection">The projection matrix</param>
/// <param name="modelview">The modelview matrix</param>
/// <returns>The coordinate in world space.</returns>
public static Coordinate Unproject(Coordinate coordinate, int[] viewport, Matrix4d projection, Matrix4d modelview)
{
var matrix = Matrix4d.Invert(Matrix4d.Mult(modelview, projection));
var source = new Vector4d(
(coordinate.DX - viewport[0]) * 2 / viewport[2] - 1,
(coordinate.DY - viewport[1]) * 2 / viewport[3] - 1,
2 * coordinate.DZ - 1,
1);
var vector = Vector4d.Transform(source, matrix);
if (Math.Abs(vector.W - 0) < 0.00001)
{
return(null);
}
var result = Vector3d.Divide(vector.Xyz, vector.W);
return(new Coordinate((decimal)result.X, (decimal)result.Y, (decimal)result.Z));
}
Vector3d GetLocation(PointF pt, float z)
{
var w2 = Width / 2.0;
var h2 = Height / 2.0;
var screen = new Vector4d((pt.X - w2) / w2, (h2 - pt.Y) / h2, z, 1);
var final = Matrix4d.Mult(_viewMatrix, _projectionMatrix);
final.Invert();
var pos = final.Tx(screen);
var p = new Vector3d(pos.X / pos.W, pos.Y / pos.W, pos.Z / pos.W);
//Console.WriteLine ("{0} -> {1}", pt, p);
return(p);
}
private void WriteNextMtx(int hnumber, sbyte bnumber, sbyte pnumber, Matrix4d testmtx, ref int ofs)
{
sbyte child = Hierarchies[hnumber][bnumber].Child;
sbyte sibling = Hierarchies[hnumber][bnumber].Sibling;
Matrix4d localmtx = testmtx;
localmtx = Matrix4d.Mult(Matrix4d.CreateTranslation(Hierarchies[hnumber][bnumber].Translation), testmtx);
localmtx = Matrix4d.Mult(Matrix4d.CreateRotationZ(MathHelper.DegreesToRadians((float)Hierarchies[hnumber][bnumber].Rotation.Z / 182.0444444f)), localmtx);
localmtx = Matrix4d.Mult(Matrix4d.CreateRotationY(MathHelper.DegreesToRadians((float)Hierarchies[hnumber][bnumber].Rotation.Y / 182.0444444f)), localmtx);
localmtx = Matrix4d.Mult(Matrix4d.CreateRotationX(MathHelper.DegreesToRadians((float)Hierarchies[hnumber][bnumber].Rotation.X / 182.0444444f)), localmtx);
if (Hierarchies[hnumber][bnumber].DisplayList != 0)
{
WriteMtxData(localmtx.M11, hnumber, ref ofs);
WriteMtxData(localmtx.M12, hnumber, ref ofs);
WriteMtxData(localmtx.M13, hnumber, ref ofs);
WriteMtxData(localmtx.M14, hnumber, ref ofs);
WriteMtxData(localmtx.M21, hnumber, ref ofs);
WriteMtxData(localmtx.M22, hnumber, ref ofs);
WriteMtxData(localmtx.M23, hnumber, ref ofs);
WriteMtxData(localmtx.M24, hnumber, ref ofs);
WriteMtxData(localmtx.M31, hnumber, ref ofs);
WriteMtxData(localmtx.M32, hnumber, ref ofs);
WriteMtxData(localmtx.M33, hnumber, ref ofs);
WriteMtxData(localmtx.M34, hnumber, ref ofs);
WriteMtxData(localmtx.M41, hnumber, ref ofs);
WriteMtxData(localmtx.M42, hnumber, ref ofs);
WriteMtxData(localmtx.M43, hnumber, ref ofs);
WriteMtxData(localmtx.M44, hnumber, ref ofs);
ofs += 0x20;
}
if (child > -1 && child != bnumber)
{
WriteNextMtx(hnumber, child, bnumber, localmtx, ref ofs);
}
if (sibling > -1 && sibling != bnumber)
{
WriteNextMtx(hnumber, sibling, pnumber, testmtx, ref ofs);
}
}
/// <summary>Gets the vector of where the mouse cursor currently is.</summary>
/// <param name="projection"></param>
/// <param name="modelView"></param>
/// <param name="offsetX">will get the coords of the pixel to the right of the mouse cursor if true</param>
/// <param name="offsetY">will get the coords of the pixel below the mouse cursor if true</param>
private static Vector3d GetMousePosition(ref Matrix4d projection, ref Matrix4d modelView, bool offsetX, bool offsetY)
{
Matrix4d view;
Matrix4d.Mult(ref modelView, ref projection, out view);
int x = Settings.Game.Mouse.X + (offsetX ? 1 : 0);
int y = Settings.Game.Height + (offsetY ? 1 : 0) - Settings.Game.Mouse.Y - 1; //invert Y, window coords are opposite
float depth = 0;
GL.ReadPixels(x, y, 1, 1, PixelFormat.DepthComponent, PixelType.Float, ref depth);
var viewPosition = new Vector4d
(
(float)x / Settings.Game.Width * 2.0f - 1.0f, //map X to -1 to 1 range
(float)y / Settings.Game.Height * 2.0f - 1.0f, //map Y to -1 to 1 range
depth * 2.0f - 1.0f, //map Z to -1 to 1 range
1.0f
);
var temp = Vector4d.Transform(viewPosition, Matrix4d.Invert(view));
return(new Vector3d(temp.X, temp.Y, temp.Z) / temp.W);
}
static int gluUnProject4(double winx, double winy, double winz, double clipw, Matrix4d modelMatrix, Matrix4d projMatrix, int[] viewport, double near, double far, ref double objx,
ref double objy, ref double objz, ref double objw)
{
Matrix4d finalMatrix;
Vector4d _in;
Vector4d _out;
finalMatrix = Matrix4d.Mult(modelMatrix, projMatrix);
//if (!__gluInvertMatrixd(finalMatrix, finalMatrix)) return(GL_FALSE);
finalMatrix.Invert();
_in.X = winx;
_in.Y = winy;
_in.Z = winz;
_in.W = clipw;
/* Map x and y from window coordinates */
_in.X = (_in.X - viewport[0]) / viewport[2];
_in.Y = (_in.Y - viewport[1]) / viewport[3];
_in.Z = (_in.Z - near) / (far - near);
/* Map to range -1 to 1 */
_in.X = _in.X * 2 - 1;
_in.Y = _in.Y * 2 - 1;
_in.Z = _in.Z * 2 - 1;
// TODO: check again (same order issue as prev. todos)
_out = Vector4d.Transform(_in, finalMatrix);
if (_out.W == 0.0)
{
return(0);
}
objx = _out.X;
objy = _out.Y;
objz = _out.Z;
objw = _out.W;
return(1);
}
/// <summary>Update the ModelView and Frustum. Done on every update cycle and before the world is initially loaded so we can preload chunks in the initial frustum.</summary>
private static void UpdateFrustum()
{
ModelView = Matrix4d.LookAt(Player.Coords.Xf, Player.Coords.Yf + Constants.PLAYER_EYE_LEVEL, Player.Coords.Zf, Player.Coords.Xf + (float)Math.Cos(Player.Coords.Direction) * (float)Math.Cos(Player.Coords.Pitch), Player.Coords.Yf + Constants.PLAYER_EYE_LEVEL + (float)Math.Sin(Player.Coords.Pitch), Player.Coords.Zf + (float)Math.Sin(Player.Coords.Direction) * (float)Math.Cos(Player.Coords.Pitch), 0, 1, 0);
Matrix4d clip;
Matrix4d.Mult(ref ModelView, ref Projection, out clip);
LeftFrustum = new Vector4d(clip.M14 + clip.M11, clip.M24 + clip.M21, clip.M34 + clip.M31, clip.M44 + clip.M41);
LeftFrustum.NormalizeFast();
RightFrustum = new Vector4d(clip.M14 - clip.M11, clip.M24 - clip.M21, clip.M34 - clip.M31, clip.M44 - clip.M41);
RightFrustum.NormalizeFast();
BottomFrustum = new Vector4d(clip.M14 + clip.M12, clip.M24 + clip.M22, clip.M34 + clip.M32, clip.M44 + clip.M42);
BottomFrustum.NormalizeFast();
TopFrustum = new Vector4d(clip.M14 - clip.M12, clip.M24 - clip.M22, clip.M34 - clip.M32, clip.M44 - clip.M42);
TopFrustum.NormalizeFast();
NearFrustum = new Vector4d(clip.M14 + clip.M13, clip.M24 + clip.M23, clip.M34 + clip.M33, clip.M44 + clip.M43);
NearFrustum.NormalizeFast();
FarFrustum = new Vector4d(clip.M14 - clip.M13, clip.M24 - clip.M23, clip.M34 - clip.M33, clip.M44 - clip.M43);
FarFrustum.NormalizeFast();
}
/// <summary>
/// A single ICP Iteration
/// </summary>
/// <param name="pointsTarget"></param>
/// <param name="pointsSource"></param>
/// <param name="PT"></param>
/// <param name="PS"></param>
/// <param name="kdTree"></param>
/// <returns></returns>
private bool Helper_ICP_Iteration(ref List <Vertex> pointsTarget, ref List <Vertex> pointsSource, List <Vertex> PT, List <Vertex> PS, KDTreeVertex kdTree, int keepOnlyPoints)
{
if (!Helper_FindNeighbours(ref pointsTarget, ref pointsSource, kdTree, keepOnlyPoints))
{
return(true);
}
Matrix4d myMatrix = Helper_FindTransformationMatrix(pointsTarget, pointsSource);
List <Vertex> myPointsTransformed = MathUtils.TransformPoints(pointsSource, myMatrix);
if (SimulatedAnnealing)
{
this.Matrix = myMatrix;
double totaldist = PointUtils.CalculateTotalDistance(pointsTarget, myPointsTransformed);
this.MeanDistance = totaldist / Convert.ToDouble(pointsTarget.Count);
//new set:
pointsSource = myPointsTransformed;
pointsTarget = Vertices.CopyVertices(PT);
}
else
{
Matrix4d.Mult(ref myMatrix, ref this.Matrix, out this.Matrix);
double totaldist = PointUtils.CalculateTotalDistance(pointsTarget, myPointsTransformed);
this.MeanDistance = totaldist / Convert.ToDouble(pointsTarget.Count);
//Debug.WriteLine("--------------Iteration: " + iter.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000"));
if (MeanDistance < this.MaximumMeanDistance) //< Math.Abs(MeanDistance - oldMeanDistance) < this.MaximumMeanDistance)
{
return(true);
}
Helper_SetNewInterationSets(ref pointsTarget, ref pointsSource, PT, PS);
}
return(false);
}
