public double Ver(DrawingContext dc, Target TargetA, Target TargetB, bool show)
{
//3D
Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, TargetA.point3D().Y - TargetB.point3D().Y, 0);// TargetA.point3D().Z - TargetB.point3D().Z);
Vector3D vectorB = new Vector3D(0, 1, 0);
//2D
//Vector3D vectorA = new Vector3D(TargetA.point2D().X - TargetB.point2D().X, TargetA.point2D().Y - TargetB.point2D().Y, 0);
//Vector3D vectorB = new Vector3D(0, TargetA.point2D().Y - TargetB.point2D().Y, 0);
double theta = Math.Abs(Vector3D.AngleBetween(vectorA, vectorB));
if (TargetA.point3D().X < TargetB.point3D().X) theta = -theta;
if (show) //show angle text
{
dc.DrawText(new FormattedText(theta.ToString("f0"),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushLemonChiffon),
new Point(TargetB.point2D().X - 35, TargetB.point2D().Y - 35));
dc.DrawLine(PenLemonChiffon, TargetA.point2D(), TargetB.point2D()); //show angle line
//dc.DrawLine(PenLemonChiffon, new Point(TargetB.point2D().X, TargetA.point2D().Y), new Point(TargetB.point2D().X, TargetB.point2D().Y)); //show Vertical line
}
return theta;
}
protected override Geometry3D CreateMesh()
{
_radius = Radius;
_position = Position;
MeshGeometry3D mesh = new MeshGeometry3D();
Point3D prevPoint = PointForAngle(0);
Vector3D normal = new Vector3D(0, 0, 1);
const int div = 180;
for (int i = 1; i <= div; ++i)
{
double angle = 2 * Math.PI / div * i;
Point3D newPoint = PointForAngle(angle);
mesh.Positions.Add(prevPoint);
mesh.Positions.Add(_position);
mesh.Positions.Add(newPoint);
mesh.Normals.Add(normal);
mesh.Normals.Add(normal);
mesh.Normals.Add(normal);
prevPoint = newPoint;
}
mesh.Freeze();
return mesh;
}
public static MeshGeometry3D CreateXY(Vector3D normal, double width, double height)
{
double w = width / 2.0;
double h = height / 2.0;
return Create(normal, new Point3D(-w, h, 0), new Point3D(-w, -h, 0), new Point3D(w, -h, 0), new Point3D(w, h, 0));
}
public trajectory_version2(Vector3D entry_point, Vector3D exit_point)
{
e_u = new Vector3D(); //e: unit vector
e_u = exit_point - entry_point;
circle_normal = new Vector3D();
e_x = new Vector3D(e_u.X, 0, e_u.Z);
e_x = e_x / e_x.Length;
e_y = new Vector3D(0, 1, 0);
circle_normal = Vector3D.CrossProduct(e_y, e_u);
circle_normal = circle_normal / circle_normal.Length;
e_v = new Vector3D();
e_v = Vector3D.CrossProduct(circle_normal, e_u);
Vector3D offset = new Vector3D();
offset = Math.Sqrt(r*r-.25 * e_u.Length*e_u.Length) * e_v/e_v.Length;
circle_center = new Vector3D(0.5 * (entry_point.X + exit_point.X), 0.5 * (entry_point.Y + exit_point.Y), 0.5 * (entry_point.Z + exit_point.Z));
circle_center = circle_center - offset;
needle_holder_position = 2 * circle_center - entry_point;
needle_holder_twist = Math.Acos(Vector3D.DotProduct((needle_holder_position - circle_center), e_x) / ((needle_holder_position - circle_center).Length * e_x.Length) );
/*
Vector3D temp = new Vector3D();
temp = needle_holder_position - circle_center;
needle_holder_twist = Math.Acos(Vector3D.DotProduct(temp, e_x) / (temp.Length * e_x.Length));
* */
print_vector(entry_point);
print_vector(exit_point);
print_vector(circle_center);
print_vector(needle_holder_position);
}
public static PointCollection GeneratePlanarTextureCoordinates(MeshGeometry3D mesh, Vector3D dir)
{
if (mesh == null)
return null;
return GeneratePlanarTextureCoordinates(mesh, mesh.Bounds, dir);
}
private void StartAnimationPrivate(double[] coord, double[] quaternion, DateTime start)
{
Tuple <TimeSpan, double[], double[]> parameters;
lock (loc)
{
parameters = queue.Dequeue();
}
startTime = start;
this.Duration = parameters.Item1;
this.RepeatBehavior = new RepeatBehavior(1);
calculator.Set(parameters.Item2, parameters.Item3);
this.Completed += Linear6DMotion_AnimationCompleted;
System.Windows.Media.Media3D.Vector3D v =
new System.Windows.Media.Media3D.Vector3D(rotationAxis[0],
rotationAxis[1], rotationAxis[2]);
angle_uniform.Axis = v;
angle_uniform.Angle = 0;
Quaternion qua = new Quaternion(quaternion[1], quaternion[2], quaternion[3], quaternion[0]);
quaternionConstRotation.Quaternion = qua;
translation.OffsetX = coord[0];
translation.OffsetY = coord[1];
translation.OffsetZ = coord[2];
From = 0;
To = 1;
animatable.BeginAnimation(dependencyProperty, this);
}
public Plant()
{
var x = new Vector3D(1, 0, 0);
var r1 = new RotateTransform3D(new AxisAngleRotation3D(x, 80));
var r2 = new RotateTransform3D(new AxisAngleRotation3D(x, -70));
var r3 = new RotateTransform3D(new AxisAngleRotation3D(x, -10));
var t1 = new TranslateTransform3D(0, 0, 0.5);
var t2 = new TranslateTransform3D(0, 0, 0.7);
var t3 = new TranslateTransform3D(0, 0, 1.0);
var s1 = new ScaleTransform3D(0.5, 0.5, 0.5);
var s2 = new ScaleTransform3D(0.3, 0.3, 0.3);
var s3 = new ScaleTransform3D(0.8, 0.8, 0.8);
var m1 = new Transform3DGroup();
m1.Children.Add(r1);
m1.Children.Add(s1);
m1.Children.Add(t1);
var m2 = new Transform3DGroup();
m2.Children.Add(r2);
m2.Children.Add(s2);
m2.Children.Add(t2);
var m3 = new Transform3DGroup();
m3.Children.Add(r3);
m3.Children.Add(s3);
m3.Children.Add(t3);
T1 = m1;
T2 = m2;
T3 = m3;
}
public static MeshGeometry3D AddPlaneToMesh(MeshGeometry3D mesh, Vector3D normal, Point3D upperLeft, Point3D lowerLeft, Point3D lowerRight, Point3D upperRight)
{
int offset = mesh.Positions.Count;
mesh.Positions.Add(upperLeft);
mesh.Positions.Add(lowerLeft);
mesh.Positions.Add(lowerRight);
mesh.Positions.Add(upperRight);
mesh.Normals.Add(normal);
mesh.Normals.Add(normal);
mesh.Normals.Add(normal);
mesh.Normals.Add(normal);
mesh.TextureCoordinates.Add(new Point(0, 0));
mesh.TextureCoordinates.Add(new Point(0, 1));
mesh.TextureCoordinates.Add(new Point(1, 1));
mesh.TextureCoordinates.Add(new Point(1, 0));
mesh.TriangleIndices.Add(offset + 0);
mesh.TriangleIndices.Add(offset + 1);
mesh.TriangleIndices.Add(offset + 2);
mesh.TriangleIndices.Add(offset + 0);
mesh.TriangleIndices.Add(offset + 2);
mesh.TriangleIndices.Add(offset + 3);
return mesh;
}
public static bool ModelRayIntersection(WW3DModel model, Point3D rayOrig, Vector3D rayDir, out Point3D hitPos, out Vector3D hitSurfaceNormal, out double rayLength)
{
rayLength = double.MaxValue;
hitPos = new Point3D();
hitSurfaceNormal = new Vector3D();
var points = model.TriangleList();
var indices = model.IndexList();
for (int i = 0; i < indices.Length/3; ++i) {
Point3D pos;
Vector3D surfaceNormal;
double distance;
if (!TriangleRayIntersect(points[indices[i * 3 + 0]], points[indices[i * 3 + 1]], points[indices[i * 3 + 2]], rayOrig, rayDir, out pos, out surfaceNormal, out distance)) {
continue;
}
if (distance < rayLength) {
hitPos = pos;
hitSurfaceNormal = surfaceNormal;
rayLength = distance;
}
}
return rayLength != double.MaxValue;
}
public BindableVector3DModel(Vector3D vector)
: this()
{
X = vector.X;
Y = vector.Y;
Z = vector.Z;
}
public static Matrix3D EulerAngleToMatrix(Vector3D pEulersAngles)
{
NewtonMatrix aNewtonMatrix = new NewtonMatrix(Matrix3D.Identity);
Newton.NewtonSetEulerAngle(new NewtonVector3(pEulersAngles).NWVector3,
aNewtonMatrix.NWMatrix);
return aNewtonMatrix.ToDirectX();
}
public Sphere(Vector center, float radius, Surface surface) : base(surface)
{
this.center = center;
this.radius = radius;
this.surface = surface;
radius2 = radius * radius;
}
/// <summary>
/// Calculates the texture for the specified model.
/// </summary>
/// <param name="model">
/// The model.
/// </param>
/// <param name="mesh">
/// The mesh.
/// </param>
public override void Calculate(TerrainModel model, MeshGeometry3D mesh)
{
var normals = MeshGeometryHelper.CalculateNormals(mesh);
var texcoords = new PointCollection();
var up = new Vector3D(0, 0, 1);
for (int i = 0; i < normals.Count; i++)
{
double slope = Math.Acos(Vector3D.DotProduct(normals[i], up)) * 180 / Math.PI;
double u = slope / 40;
if (u > 1)
{
u = 1;
}
if (u < 0)
{
u = 0;
}
texcoords.Add(new Point(u, u));
}
this.TextureCoordinates = texcoords;
this.Material = MaterialHelper.CreateMaterial(this.Brush);
}
//two kinect vector to system 3d vector between those two vectors .
public System.Windows.Media.Media3D.Vector3D vectorbetween23dvectors(Microsoft.Research.Kinect.Nui.Vector vec1, Microsoft.Research.Kinect.Nui.Vector vec2)
{
System.Windows.Media.Media3D.Vector3D vect1 = new System.Windows.Media.Media3D.Vector3D(vec1.X, vec1.Y, vec1.Z);
System.Windows.Media.Media3D.Vector3D vect2 = new System.Windows.Media.Media3D.Vector3D(vec2.X, vec2.Y, vec2.Z);
System.Windows.Media.Media3D.Vector3D vect = vect1 - vect2;
return(vect);
}
public void ZoomToBounds(SharpDX.BoundingBox bounds, double animationTime = 0)
{
if (bounds.Size.IsZero)
{
return;
}
var pcam = Viewport.Camera as Helix.PerspectiveCamera;
var center = bounds.Center.ToPoint3D();
var radius = bounds.Size.Length() / 2;
double disth = radius / Math.Tan(0.5 * pcam.FieldOfView * Math.PI / 180);
double vfov = pcam.FieldOfView / Viewport.ActualWidth * Viewport.ActualHeight;
double distv = radius / Math.Tan(0.5 * vfov * Math.PI / 180);
double adjust = distv > disth ? 0.75 : 1;
double dist = Math.Max(disth, distv) * adjust;
var dir = (bounds.Size.X > bounds.Size.Y * 1.5)
? new Media3D.Vector3D(0, -Math.Sign(center.Y), 0)
: new Media3D.Vector3D(-Math.Sign(center.X), 0, 0);
if (dir.Length == 0)
{
dir = new Media3D.Vector3D(-1, 0, 0);
}
pcam.LookAt(center, dir * dist, Viewport.ModelUpDirection, animationTime);
}
//Find the line of intersection between two planes.
//The inputs are two game objects which represent the planes.
//The outputs are a point on the line and a vector which indicates it's direction.
internal static bool PlanesIntersection(Plane3D plane1, Plane3D plane2, out StraightLine intersection)
{
var linePoint = new WM.Point3D();
var lineVec = new WM.Vector3D();
//Get the normals of the planes.
var plane1Normal = plane1.NormalVector;
var plane2Normal = plane2.NormalVector;
//We can get the direction of the line of intersection of the two planes by calculating the
//cross product of the normals of the two planes. Note that this is just a direction and the line
//is not fixed in space yet.
lineVec = WM.Vector3D.CrossProduct(plane1Normal, plane2Normal);
//Next is to calculate a point on the line to fix it's position. This is done by finding a vector from
//the plane2 location, moving parallel to it's plane, and intersecting plane1. To prevent rounding
//errors, this vector also has to be perpendicular to lineDirection. To get this vector, calculate
//the cross product of the normal of plane2 and the lineDirection.
var ldir = WM.Vector3D.CrossProduct(plane2Normal, lineVec);
var numerator = WM.Vector3D.DotProduct(plane1Normal, ldir);
//Prevent divide by zero.
if (Math.Abs(numerator) > 0.000001)
{
var plane1ToPlane2 = plane1.PointOnPlane - plane2.PointOnPlane;
var t = WM.Vector3D.DotProduct(plane1Normal, plane1ToPlane2) / numerator;
linePoint = plane2.PointOnPlane + t * ldir;
intersection = new StraightLine(ref linePoint, ref lineVec);
return(true);
}
intersection = default(StraightLine);
return(false);
}
internal static double AngleBetween(WM.Vector3D va, WM.Vector3D vb, WM.Vector3D vn, bool correctTo90_90 = false)
{
var angle = Math.Atan2(WM.Vector3D.DotProduct(WM.Vector3D.CrossProduct(vb, va), vn), WM.Vector3D.DotProduct(va, vb));
if (correctTo90_90)
{
if (angle.IsEqual(Math.PI) || angle.IsEqual(-Math.PI))
{
return(0);
}
if (angle > 0)
{
while (angle > Math.PI / 2)
{
angle -= Math.PI / 2;
}
}
else
{
while (angle < -Math.PI / 2)
{
angle += Math.PI / 2;
}
}
}
return(angle);
}
public static double angle(Atom a, Atom b)
{
Vector3D va = new Vector3D(a.getX(), a.getY(), a.getZ());
Vector3D vb = new Vector3D(b.getX(), b.getY(), b.getZ());
return Vector3D.AngleBetween(va, vb);
}
private Color getNaturalColor(Thing thing, Vector pos, Vector norm, Vector rd, Scene scene)
{
var addLight = new Func <Color, Light, Color>((col, light) =>
{
var ldis = light.pos - pos;
var livec = Help.norm(ldis);
var neatIsect = testRay(
new Ray {
start = pos, dir = livec
}, scene);
var isInShadow = !float.IsNaN(neatIsect) && neatIsect <= (float)ldis.LengthSquared;
if (isInShadow)
{
return(col);
}
else
{
var illum = (float)Vector.DotProduct(livec, norm);
var lcolor = (illum > 0)
? illum * light.color
: Color.defaultColor;
var specular = Vector.DotProduct(livec, Help.norm(rd));
var scolor = (specular > 0)
? (float)Math.Pow(specular, thing.surface.roughness) * light.color
: Color.defaultColor;
return(col + ((thing.surface.diffuse(pos) * lcolor) +
(thing.surface.specular(pos) * scolor)));
}
});
return(scene.lights.Aggregate(Color.defaultColor, addLight));
}
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 override void Load()
{
IsEnabled = true;
try
{
_wiimote = new Wiimote();
_wiimote.Connect();
_wiimote.InitializeMotionPlus();
_wiimote.WiimoteChanged += wiimote_WiimoteChanged;
_wiimote.SetRumble(true);
_wiimote.SetLEDs(true, false, false, true);
Thread.Sleep(40);
_wiimote.SetRumble(false);
RawPosition = new Vector3D();
}
catch (Exception exc)
{
Logger.Instance.Error(exc.Message, exc);
try
{
_wiimote.SetLEDs(false, false, false, false);
}
catch (Exception exception)
{
Logger.Instance.Error(exception.Message, exception);
}
IsEnabled = false;
}
}
public static void SetToVector(this TranslateTransform3D translateTransform3D, Vector3D vector3D)
{
Contract.Requires(translateTransform3D != null);
translateTransform3D.OffsetX = vector3D.X;
translateTransform3D.OffsetY = vector3D.Y;
translateTransform3D.OffsetZ = vector3D.Z;
}
public double? Test(Vector3D renderPoint, Vector3D direction)
{
var lineToCircle = renderPoint - CenterPoint;
var B = 2 * Vector3D.DotProduct(direction, lineToCircle);
var C = lineToCircle.LengthSquared - Math.Pow(Radius, 2);
var determinate = Math.Pow(B, 2) - 4 * C;
if (determinate >= 0)
{
var diff = Math.Sqrt(determinate);
var tintersect1 = (-B - diff) / 2;
var tintersect2 = (-B + diff) / 2;
if (tintersect1 > tintersect2)
{
var temp = tintersect2;
tintersect2 = tintersect1;
tintersect1 = temp;
}
if (tintersect1 < 0)
{
tintersect1 = tintersect2;
}
if (tintersect1 >= 0)
{
return tintersect1;
}
}
return null;
}
public void CreateSphere(Vector3D pRadius, Matrix3D pOffsetMatrix)
{
NewtonMatrix aMatrix = new NewtonMatrix(pOffsetMatrix);
m_Handle = Newton.NewtonCreateSphere(m_World.Handle, (float)pRadius.X, (float)pRadius.Y, (float)pRadius.Z, aMatrix.NWMatrix);
CHashTables.Collision.Add(m_Handle, this);
}
/// <summary>
/// Gets intersection of two planes
/// </summary>
/// <param name="plane1">The first plane</param>
/// <param name="plane2">The second plane</param>
/// <param name="pointOnLine">Point on line where <paramref name="plane1"/> and <paramref name="plane1"/> are intersecting</param>
/// <param name="dirVect">Direction vector of the intersection</param>
/// <returns>True if intersection exists</returns>
public static bool GetIntersection(Plane3D plane1, Plane3D plane2, out WM.Point3D pointOnLine, out WM.Vector3D dirVect)
{
#if DEBUG
// Normal vectors should be normalized
WM.Vector3D testVect1 = plane1.NormalVector;
testVect1.Normalize();
WM.Vector3D testVect2 = plane2.NormalVector;
testVect2.Normalize();
Debug.Assert(WM.Vector3D.DotProduct(plane1.NormalVector, plane2.NormalVector).IsEqual(WM.Vector3D.DotProduct(testVect1, testVect2)));
#endif
double normalDot = WM.Vector3D.DotProduct(plane1.NormalVector, plane2.NormalVector);
double D = (1 - normalDot * normalDot);
if (D.IsZero())
{
pointOnLine = new WM.Point3D(double.NaN, double.NaN, double.NaN);
dirVect = new WM.Vector3D(double.NaN, double.NaN, double.NaN);
return(false);
}
double h1 = -1 * plane1.GetD();
double h2 = -1 * plane2.GetD();
double c1 = (h1 - h2 * normalDot) / D;
double c2 = (h2 - h1 * normalDot) / D;
dirVect = WM.Vector3D.CrossProduct(plane1.NormalVector, plane2.NormalVector);
dirVect.Normalize();
pointOnLine = (WM.Point3D)(c1 * plane1.NormalVector + c2 * plane2.NormalVector);
return(true);
}
public Vector3D NormalAt(Vector3D intersectionPoint)
{
//Assert actually an intersection?
var surfaceNormal = intersectionPoint - CenterPoint;
surfaceNormal.Normalize();
return surfaceNormal;
}
private ModelVisual3D makeTriangle(double x1, double y1, double z1, double x2, double y2, double z2, double x3, double y3, double z3)
{
MeshGeometry3D triangleMesh = new MeshGeometry3D();
Point3D point0 = new Point3D(x1, y1, z1);
Point3D point1 = new Point3D(x2, y2, z2);
Point3D point2 = new Point3D(x3, y3, z3);
triangleMesh.Positions.Add(point0);
triangleMesh.Positions.Add(point1);
triangleMesh.Positions.Add(point2);
triangleMesh.TriangleIndices.Add(0); // Важно - порядок обхода
triangleMesh.TriangleIndices.Add(2);
triangleMesh.TriangleIndices.Add(1);
Vector3D normal = new Vector3D(0, 1, 0);
triangleMesh.Normals.Add(normal);
triangleMesh.Normals.Add(normal);
triangleMesh.Normals.Add(normal);
Material material = new DiffuseMaterial(new SolidColorBrush(Colors.Red));
return new ModelVisual3D { Content = new GeometryModel3D(triangleMesh, material) };
}
// Transfrom that moves the world to a camera coordinate system
// where the camera is at the origin looking down the negative z
// axis and y is up.
//
// NOTE: We consider camera.Transform to be part of the view matrix.
//
internal static Matrix3D CreateViewMatrix(Transform3D transform, ref Point3D position, ref Vector3D lookDirection, ref Vector3D upDirection)
{
Vector3D zaxis = -lookDirection;
zaxis.Normalize();
Vector3D xaxis = Vector3D.CrossProduct(upDirection, zaxis);
xaxis.Normalize();
Vector3D yaxis = Vector3D.CrossProduct(zaxis, xaxis);
Vector3D positionVec = (Vector3D) position;
double cx = -Vector3D.DotProduct(xaxis, positionVec);
double cy = -Vector3D.DotProduct(yaxis, positionVec);
double cz = -Vector3D.DotProduct(zaxis, positionVec);
Matrix3D viewMatrix = new Matrix3D(
xaxis.X, yaxis.X, zaxis.X, 0,
xaxis.Y, yaxis.Y, zaxis.Y, 0,
xaxis.Z, yaxis.Z, zaxis.Z, 0,
cx, cy, cz, 1);
PrependInverseTransform(transform, ref viewMatrix);
return viewMatrix;
}
/// <summary>
/// Helper method creates a triangle fan to close the ends of the cylinder.
/// </summary>
void CreateCap(int tessellation, float height, float radius, Vector3D normal)
{
// Create cap indices.
for (int i = 0; i < tessellation - 2; i++)
{
if (normal.Y > 0)
{
AddIndex(CurrentVertex);
AddIndex(CurrentVertex + (i + 1) % tessellation);
AddIndex(CurrentVertex + (i + 2) % tessellation);
}
else
{
AddIndex(CurrentVertex);
AddIndex(CurrentVertex + (i + 2) % tessellation);
AddIndex(CurrentVertex + (i + 1) % tessellation);
}
}
// Create cap vertices.
for (int i = 0; i < tessellation; i++)
{
Vector3D vertex = GetCircleVector(i, tessellation) * radius +
normal * height;
Point3D vertexPoint = new Point3D(vertex.X, vertex.Y, vertex.Z);
AddVertex(vertexPoint, normal);
}
}
public WWLineSegment(Point3D startPos, Vector3D dirNormalized, double length, double intensity)
{
StartPos = startPos;
Direction = dirNormalized;
Length = length;
Intensity = intensity;
}
public WWFirCoefficient(double delaySecond, Vector3D soundDir, double gain, bool isDirect)
{
DelaySecond = delaySecond;
SoundDirection = soundDir;
Gain = gain;
IsDirect = isDirect;
}
public SpatialEntity(string name, string model, Vector3D position, Quaternion rotation)
{
Name = name;
Model = model;
Position = position;
Rotation = rotation;
}
public MainViewModel()
{
EffectsManager = new DefaultEffectsManager();
// titles
Title = "Viewer";
SubTitle = "Marine Technology";
// camera setup
Camera = new HelixToolkit.Wpf.SharpDX.PerspectiveCamera
{
Position = new Point3D(0, -5, 5),
LookDirection = new Vector3D(0, 10, 5),//z kamerą jest problem przy przybliżaniu prawdopodobnie przez ten LookDirection albo sposób poruszania kamerą
UpDirection = new Vector3D(0, 0, 1),
FarPlaneDistance = 5000000
};
// setup lighting
AmbientLightColor = Colors.DimGray;
DirectionalLightColor = Colors.White; // światełko musi być
DirectionalLightDirection = Camera.LookDirection;
MainView();
AddPointCloud();
UpXCommand = new RelayCommand(x => { UpDirection = new Vector3D(1, 0, 0); });
UpYCommand = new RelayCommand(x => { UpDirection = new Vector3D(0, 1, 0); });
UpZCommand = new RelayCommand(x => { UpDirection = new Vector3D(0, 0, 1); });
}
//trajectory() in MATLAB; calculation position of end effector and ideal orientation of the needle
public dof4 end_effector(Vector3D forearm_orientation, double delta_theta)
{
needle_holder_twist = needle_holder_twist + t_incr;
update_needle_holder_position();
dof4 needle_holder;
needle_holder.pos = needle_holder_position;
needle_holder.twist = needle_holder_twist;
print_vector(needle_holder_position);
print_double(needle_holder_twist);
/*
set_circle_center(); // calculating center of needle
// calculating position
DOF.pos = new Vector3D(xc + r * Math.Sin(t), yc + r * Math.Cos(t), zc);
// calculation orientation
Vector3D centric1 = new Vector3D(r * Math.Sin(t), r * Math.Cos(t), 0);
Vector3D centric2 = new Vector3D(r * Math.Sin(t + t_incr), r * Math.Cos(t + t_incr), 0);
Vector3D normal = new Vector3D();
//Vector3D ideal_needle_orientation = new Vector3D();
normal = Vector3D.CrossProduct(centric1, centric2); // normal of circle plane
ideal_needle_orientation = Vector3D.CrossProduct(centric1, normal); // which is the tangent of the path
Vector3D optimal_needle_orientation = new Vector3D();
optimal_needle_orientation = minimizer(forearm_orientation);
DOF.ori = optimal_needle_orientation;
*/
return needle_holder;
}
/// <summary>
/// LookAtRH
/// </summary>
/// http://msdn.microsoft.com/en-us/library/bb281711(v=vs.85).aspx
/// <returns></returns>
public static Matrix3D SetViewMatrix(Point3D cameraPosition, Vector3D lookDirection, Vector3D upDirection)
{
// Normalize vectors:
lookDirection.Normalize();
upDirection.Normalize();
double dotProduct = Vector3D.DotProduct(lookDirection, upDirection);
// Define vectors, XScale, YScale, and ZScale:
double denom = Math.Sqrt(1 - Math.Pow(dotProduct, 2));
Vector3D XScale = Vector3D.CrossProduct(lookDirection, upDirection) / denom;
Vector3D YScale = (upDirection - dotProduct * lookDirection) / denom;
Vector3D ZScale = lookDirection;
// Construct M matrix:
Matrix3D M = new Matrix3D()
{
M11 = XScale.X, M12 = YScale.X, M13 = ZScale.X,
M21 = XScale.Y, M22 = YScale.Y, M23 = ZScale.Y,
M31 = XScale.Z, M32 = YScale.Z, M33 = ZScale.Z
};
// Translate the camera position to the origin:
Matrix3D translateMatrix = new Matrix3D();
translateMatrix.Translate(new Vector3D(-cameraPosition.X, -cameraPosition.Y, -cameraPosition.Z));
// Define reflect matrix about the Z axis:
Matrix3D reflectMatrix = new Matrix3D();
reflectMatrix.M33 = -1;
// Construct the View matrix:
Matrix3D viewMatrix = translateMatrix * M * reflectMatrix;
return viewMatrix;
}
/// <summary>
/// Create a look at matrix using U, V, and N vectors
/// </summary>
/// <param name="eye">Location of the eye</param>
/// <param name="lookDirection">Direction to look</param>
/// <param name="up">The up vector</param>
/// <returns>Transform matrix from world space to camera space.</returns>
public static Matrix3D LookAt(Vector3D eye, Vector3D lookDirection, Vector3D up)
{
var n = lookDirection;
n.Normalize();
var v = up - (Vector3D.DotProduct(up, n) * n);
v.Normalize();
// The "-" below makes this be a right-handed uvn space so we aren't
// negating the x component in SensorToScreenCoordinatesTransform.
// It also makes SensorToScreenPositionTransform give an immediately
// usable transform without having to flip the X.
var u = -Vector3D.CrossProduct(n, v);
var lookAt = new Matrix3D(
u.X,
v.X,
n.X,
0,
u.Y,
v.Y,
n.Y,
0,
u.Z,
v.Z,
n.Z,
0,
-Vector3D.DotProduct(u, eye),
-Vector3D.DotProduct(v, eye),
-Vector3D.DotProduct(n, eye),
1);
return lookAt;
}
public static void Move(this Polygon3D p, WM.Vector3D dir)
{
for (var i = p.Count - 1; i >= 0; --i)
{
p[i] += dir;
}
}
/// <summary> Adds a point combined. </summary>
///
/// <param name="point"> The point. </param>
/// <param name="mesh"> The mesh. </param>
/// <param name="normal"> The normal. </param>
public static void addPointCombined(Point3D point, MeshGeometry3D mesh, Vector3D normal)
{
bool found = false;
int i = 0;
foreach (Point3D p in mesh.Positions)
{
if (p.Equals(point))
{
found = true;
mesh.TriangleIndices.Add(i);
mesh.Positions.Add(point);
mesh.Normals.Add(normal);
break;
}
i++;
}
if (!found)
{
mesh.Positions.Add(point);
mesh.TriangleIndices.Add(mesh.TriangleIndices.Count);
mesh.Normals.Add(normal);
}
}
/// <summary>
///
/// </summary>
/// <param name="origin"></param>
/// <param name="dirX"></param>
/// <returns></returns>
public static Matrix44 GetLCSMatrix(WM.Point3D origin, WM.Vector3D dirX)
{
dirX.Normalize();
Vector3D localX = new Vector3D(dirX.X, dirX.Y, dirX.Z);
Vector3D localZ = new Vector3D(0, 0, 1);
if (GeomOperation.IsCollinear(localZ, localX, MathConstants.ZeroWeak)) //MathConstants.ZeroGeneral))
{
// When X is vertical then Z goes in X dir or -X
//localZ = new Vector3D(0, 1, 0);
if (localX.DirectionZ > 0)
{
localZ = new Vector3D(-1, 0, 0);
}
else
{
localZ = new Vector3D(1, 0, 0);
}
}
//Vector3D localY = (localZ * localX).Normalize;
//Matrix44 matrix = new Matrix44(origin, localX, localY);
Vector3D localY = (localZ * localX);
localZ = (localX * localY).Normalize;
Matrix44 matrix = new Matrix44(origin, localX.Normalize, localY.Normalize, localZ);
return(matrix);
}
public void CalculateJointForces(Point3D[, ,] framePoints)
{
int dim = 2;
jointForces = new Vector3D[dim, dim, dim];
for (int i = 0; i < dim; i++)
{
for (int j = 0; j < dim; j++)
{
for (int k = 0; k < dim; k++)
{
Vector3D P1;//kostka
Vector3D P2;//ramka
P1 = X[3 * i, 3 * j, 3 * k];
Point3D p = framePoints[i, j, k];
P2 = new Vector3D(p.X, p.Y, p.Z);
if (IsDampingActive)
CollisionChecker.TrimPoint(ref P2);
Vector3D V1 = V0[3 * i, 3 * j, 3 * k];
Vector3D I0 = new Vector3D(0, 0, 0); //długość spoczynkowa sprężynki połączonej z ramką ma być 0
Vector3D diff = P1 - P2;
if (Math.Sqrt(diff.X * diff.X + diff.Y * diff.Y + diff.Z * diff.Z) < 0.00001)
jointForces[i, j, k] = new Vector3D(0, 0, 0);
else
jointForces[i, j, k] = springFrame.GetCurrentForce(P1, P2, I0, V1);
}
}
}
}
public static bool TriangleRayIntersect(Point3D p0, Point3D p1, Point3D p2, Point3D rayOrig, Vector3D rayDir, out Point3D hitPos, out Vector3D surfaceNormal, out double distance)
{
var edge01 = p1 - p0;
var edge02 = p2 - p0;
surfaceNormal = Vector3D.CrossProduct(edge01, edge02);
surfaceNormal.Normalize();
hitPos = new Point3D();
distance = double.MaxValue;
var p = Vector3D.CrossProduct(rayDir, edge02);
var det = Vector3D.DotProduct(edge01, p);
if (det < float.Epsilon) {
// レイとトライアングルが平行 or backface
return false;
}
var tvec = rayOrig - p0;
var u = Vector3D.DotProduct(tvec,p);
if (u < 0 || det < u) {
return false;
}
var qvec = Vector3D.CrossProduct(tvec, edge01);
var v = Vector3D.DotProduct(rayDir, qvec);
if (v < 0 || det < u+v) {
return false;
}
distance = Vector3D.DotProduct(edge02, qvec) / det;
hitPos = rayOrig + distance * rayDir;
return true;
}
public double Hor(DrawingContext dc, Target TargetA, Target TargetB ,bool show)
{
//3D
Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, TargetA.point3D().Y - TargetB.point3D().Y, TargetA.point3D().Z - TargetB.point3D().Z);
Vector3D vectorB = new Vector3D(0, 1, 0);
//2D
//Vector3D vectorA = new Vector3D(TargetA.point2D().X - TargetB.point2D().X, TargetA.point2D().Y - TargetB.point2D().Y, 0);
//Vector3D vectorB = new Vector3D(1, 0, 0);
double theta = Math.Abs(Vector3D.AngleBetween(vectorA, vectorB));
theta = 90 - theta;
//if (TargetA.point3D().Y < TargetB.point3D().Y) theta = -theta;
if (show) //show angle text
{
dc.DrawText(new FormattedText(theta.ToString("f0"),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushDeepSkyBlue),
new Point(TargetB.point2D().X - 35, TargetB.point2D().Y - 35));
dc.DrawLine(PenDeepSkyBlue, TargetA.point2D(), TargetB.point2D()); //show angle line
dc.DrawLine(PenDeepSkyBlue, new Point(TargetA.point2D().X, TargetB.point2D().Y), TargetB.point2D());
}
return theta;
}
public Camera(Vector pos, Vector lookAt)
{
var down = new Vector(0.0, -1.0, 0.0);
forward = Help.norm(lookAt - pos);
right = 1.5 * Help.norm(Vector.CrossProduct(forward, down));
up = 1.5 * Help.norm(Vector.CrossProduct(forward, right));
}
/// <summary>
///
/// </summary>
/// <param name="vect"></param>
/// <param name="testedVect"></param>
/// <param name="tolerance"></param>
/// <returns></returns>
public static bool IsParallel(this System.Windows.Media.Media3D.Vector3D vect, System.Windows.Media.Media3D.Vector3D testedVect, double tolerance = 1e-8)
{
vect.Normalize();
testedVect.Normalize();
double dot = Math.Abs(WM.Vector3D.DotProduct(vect, testedVect));
return(dot.IsEqual(1, tolerance));
}
/// <summary>
/// Gets direction of the begin and end point of the segment (normalized)
/// </summary>
/// <param name="src">Segment</param>
/// <returns>Direction</returns>
public static WM.Vector3D GetDirection(this ISegment3D src)
{
var temp = new WM.Vector3D(src.EndPoint.X - src.StartPoint.X,
src.EndPoint.Y - src.StartPoint.Y,
src.EndPoint.Z - src.StartPoint.Z);
temp.Normalize();
return(temp);
}
public void Move(SWMM.Vector3D step)
{
SWMM.Vector3D v = GetMoveDirection(_positionHandle);
var v2 = _matrix.Transform(v);
var dp = SWMM.Vector3D.DotProduct(step, v2);
var v3 = v2 * dp;
_positionHandle.Move(v3.X, v3.Y, v3.Z);
}
/// <summary>
/// Creates a new instance of Plane3D, that is given by 3 points.
/// </summary>
/// <param name="p1">First point laying in the plane. This one is taken as origin point.</param>
/// <param name="p2">Second point laying in the plane.</param>
/// <param name="p3">Third point laying in the plane.</param>
public Plane3D(WM.Point3D p1, WM.Point3D p2, WM.Point3D p3)
{
point = p1;
var v1 = p2 - p1;
var v2 = p3 - p1;
normal = WM.Vector3D.CrossProduct(v1, v2);
normal.Normalize();
}
public static double GetArea(this Point3D[] points, out System.Windows.Media.Media3D.Vector3D normal)
{
System.Windows.Media.Media3D.Vector3D v1 = points[1] - points[0];
System.Windows.Media.Media3D.Vector3D v2 = points[2] - points[0];
System.Windows.Media.Media3D.Vector3D n = System.Windows.Media.Media3D.Vector3D.CrossProduct(v1, v2);
double a = n.Length / 2;
n.Normalize();
normal = n;
return a;
}
public RotateEventHandler(MVMIH.IRotationHandle rotationHandle)
{
_rotationHandle = rotationHandle;
_matrix = GetTranslateTransformation(rotationHandle);
_eleRotMatrix = Converters.StaticTransformationConverter.Convert((rotationHandle as IMachineElement).Parent.Parent.Transformation);
_rotationDirection = _eleRotMatrix.Transform(GetRotationDirection(rotationHandle));
_rotationCenter = _eleRotMatrix.Transform(GetRotationCenter((rotationHandle as IMachineElement).Parent.Parent));
_rotationHandle.StartRotate();
}
internal static WM.Vector3D Multiply(Matrix44 matrix, WM.Vector3D vector)
{
WM.Vector3D resVect = new WM.Vector3D
{
X = (matrix.matrixElement[0, 0] * vector.X) + (matrix.matrixElement[0, 1] * vector.Y) + (matrix.matrixElement[0, 2] * vector.Z),
Y = (matrix.matrixElement[1, 0] * vector.X) + (matrix.matrixElement[1, 1] * vector.Y) + (matrix.matrixElement[1, 2] * vector.Z),
Z = (matrix.matrixElement[2, 0] * vector.X) + (matrix.matrixElement[2, 1] * vector.Y) + (matrix.matrixElement[2, 2] * vector.Z)
};
return(resVect);
}
public void SendToolMoveMessage(int toolId, Point3D position, Vector3D direction, double length, double radius)
{
Messenger.Default.Send(new ToolMoveMessage()
{
Position = position,
Direction = direction,
Length = length,
Radius = radius
});
}
internal static WM.Vector3D Multiply(WM.Vector3D vector, Matrix44 matrix)
{
WM.Vector3D newPoint = new WM.Vector3D
{
X = (vector.X * matrix.matrixElement[0, 0]) + (vector.Y * matrix.matrixElement[1, 0]) + (vector.Z * matrix.matrixElement[2, 0]),
Y = (vector.X * matrix.matrixElement[0, 1]) + (vector.Y * matrix.matrixElement[1, 1]) + (vector.Z * matrix.matrixElement[2, 1]),
Z = (vector.X * matrix.matrixElement[0, 2]) + (vector.Y * matrix.matrixElement[1, 2]) + (vector.Z * matrix.matrixElement[2, 2])
};
return(newPoint);
}
public override Color diffuse(Vector pos)
{
if (Math.Abs((pos.Z + pos.X) % 2) > 1e-6)
{
return(Color.white);
}
else
{
return(Color.black);
}
}
public override float reflect(Vector pos)
{
if (Math.Abs((Math.Floor(pos.Z + pos.X)) % 2) > 1e-6)
{
return(0.1f);
}
else
{
return(0.7f);
}
}
public TriangleMeshAdapater(MFnMesh mesh)
{
MIntArray indices = new MIntArray();
MIntArray triangleCounts = new MIntArray();
MPointArray points = new MPointArray();
mesh.getTriangles(triangleCounts, indices);
mesh.getPoints(points);
// Get the triangle indices
Indices = new Int32Collection((int)indices.length);
for (int i = 0; i < indices.length; ++i)
{
Indices.Add(indices[i]);
}
// Get the control points (vertices)
Points = new Point3DCollection((int)points.length);
for (int i = 0; i < (int)points.length; ++i)
{
MPoint pt = points[i];
Points.Add(new Point3D(pt.x, pt.y, pt.z));
}
// Get the number of triangle faces and polygon faces
Debug.Assert(indices.length % 3 == 0);
int triFaces = (int)indices.length / 3;
int polyFaces = mesh.numPolygons;
// We have normals per polygon, we want one per triangle.
Normals = new Vector3DCollection(triFaces);
int nCurrentTriangle = 0;
// Iterate over each polygon
for (int i = 0; i < polyFaces; ++i)
{
// Get the polygon normal
var maya_normal = new MVector();
mesh.getPolygonNormal((int)i, maya_normal);
System.Windows.Media.Media3D.Vector3D normal = new System.Windows.Media.Media3D.Vector3D(maya_normal.x, maya_normal.y, maya_normal.z);
// Iterate over each tri in the current polygon
int nTrisAtFace = triangleCounts[i];
for (int j = 0; j < nTrisAtFace; ++j)
{
Debug.Assert(nCurrentTriangle < triFaces);
Normals.Add(normal);
nCurrentTriangle++;
}
}
Debug.Assert(nCurrentTriangle == triFaces);
}
/// <summary>
/// Calculates intersection of the plane and line which is defined by direction vector <paramref name="l"/> and point <paramref name="l0"/>
/// </summary>
/// <param name="src">Plane</param>
/// <param name="l0">Point on line</param>
/// <param name="l">Direction vector of line</param>
/// <returns>Found intersection. If intersection doesn't NaN is set</returns>
public static WM.Point3D GetIntersection(this Plane3D src, WM.Point3D l0, WM.Vector3D l)
{
double d = WM.Vector3D.DotProduct(src.NormalVector, l);
if (d.IsZero())
{
return(new WM.Point3D(double.NaN, double.NaN, double.NaN));
}
double dd = WM.Vector3D.DotProduct((src.PointOnPlane - l0), src.NormalVector) / d;
return(l0 + dd * l);
}
private void Set()
{
double[] quater1 = calculator.QuaternionBegin;
Quaternion qua = new Quaternion(quater1[1], quater1[2], quater1[3], quater1[0]);
SetProgressTime(currentTime);
quaternionConstRotation.Quaternion = qua;
System.Windows.Media.Media3D.Vector3D v =
new System.Windows.Media.Media3D.Vector3D(rotationAxis[0],
rotationAxis[1], rotationAxis[2]);
angle_uniform.Axis = v;
From = 0;
To = 1;
}
private Color shade(Intersection isect, Scene scene, float depth)
{
var d = isect.ray.dir;
var pos = isect.dist * d + isect.ray.start;
var normal = isect.thing.normal(pos);
var reflectDir = d - 2 * (Vector.DotProduct(normal, d) * normal);
var naturalColor = Color.background +
getNaturalColor(isect.thing, pos, normal, reflectDir, scene);
var reflectedColor = (depth >= maxDepth)
? Color.grey
: getReflectionColor(isect.thing, pos, normal, reflectDir, scene, depth);
return(naturalColor + reflectedColor);
}
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 void Rotate(System.Windows.Media.Media3D.Vector3D axis, double angle)
{
var m = viewport3D.Children[viewport3D.Children.Count - 1].Transform as Transform3DGroup;
var _rotation = (m.Children[2] as RotateTransform3D).Rotation as AxisAngleRotation3D;
var q = new System.Windows.Media.Media3D.Quaternion(_rotation.Axis, _rotation.Angle);
var delta = new System.Windows.Media.Media3D.Quaternion(axis, angle);
q *= delta;
_rotation.Axis = q.Axis;
_rotation.Angle = q.Angle;
m.Children[2] = new RotateTransform3D(_rotation);
}
