public Line3D(Point3D begin, Point3D end) { Begin = begin; End = end; Direction = end - begin; IsEmpty = Direction.IsEmpty; }
public void RayTest(Ray ray, Point3D point, double distance) { { Assert.AreEqual(ray.Origin, new Point3D(0, 0, 0)); Assert.AreEqual(ray.Direction, point); // Console.WriteLine("Ray: " + ray); } }
/// <summary> /// повернуть фрейм вокруг горизонтальной оси (y) /// </summary> /// <param name="frame">положение сонара</param> /// <param name="angle">угол поворота</param> /// <returns>вектор - новое направление</returns> public static Frame3D VerticalFrameRotation(Frame3D frame, Angle angle) { Matrix m = frame.GetMatrix(); //var right = new Point3D(m[0, 1], m[1, 1], m[2, 1]); var right = new Point3D(0,-1,0); Frame3D rotated = frame.Apply(Frame3D.DoRotate(right, angle)); return rotated; }
public static Angle AngleBetweenVectors(Point3D firstVector, Point3D secondVector) { if(firstVector.IsEmpty || secondVector.IsEmpty) throw new ArgumentException("Vectors cannot be null-vector when calculating angle"); var cos = firstVector.MultiplyScalar(secondVector) / (firstVector.Norm() * secondVector.Norm()); if(Math.Abs(cos) > 1) cos = Math.Sign(cos) * 1; return Acos(cos); }
public static Plane FromNormalVector(Point3D center, Point3D normalVector) { var some = new Point3D(1, 1, 1); if (Geometry.AreCollinear(some, normalVector)) some = new Point3D(1, 2, 1); Point3D bas1 = Geometry.Orthonorm(some, normalVector); return new Plane(center, center + bas1, center + bas1.MultiplyVector(normalVector)); }
/// <summary> /// повернуть фрейм вокруг вертикальной оси (z) /// </summary> /// <param name="frame">положение сонара</param> /// <param name="angle">угол поворота</param> /// <returns>вектор - новое направление</returns> public static Frame3D HorisontalFrameRotation(Frame3D frame, Angle angle) { Matrix m = frame.GetMatrix(); //var up = new Point3D(m[0, 2], m[1, 2], m[2, 2]); var up = new Point3D(0,0,1); Frame3D rotated = frame.Apply(Frame3D.DoRotate(up, angle)); return rotated; }
public static Point3D HorisontalRotation(Frame3D frame, Angle angle) { Matrix m = frame.GetMatrix(); var up = new Point3D(m[0, 2], m[1, 2], m[2, 2]); Frame3D rotated = frame.Apply(Frame3D.DoRotate(up, angle)); Matrix n = rotated.GetMatrix(); var rotatedFront = new Point3D(n[0, 0], n[1, 0], n[2, 0]); return rotatedFront; }
public static Point3D VerticalRotation(Frame3D frame, Angle angle) { Matrix m = frame.GetMatrix(); var right = new Point3D(m[0, 1], m[1, 1], m[2, 1]); Frame3D rotated = frame.Apply(Frame3D.DoRotate(right, angle)); Matrix n = rotated.GetMatrix(); var rotatedFront = new Point3D(n[0, 0], n[1, 0], n[2, 0]); return rotatedFront; }
public Plane(Point3D v1, Point3D v2, Point3D v3) { V1 = v1; V2 = v2; V3 = v3; Center = v1; Basis1 = (v2 - v1).Normalize(); Basis2 = Geometry.Orthonorm(v3 - v1, Basis1); Normal = Basis1.MultiplyVector(Basis2).Normalize(); }
public void Directional() { var direction = new Point3D(0, 0, -1); Light l = new LightSettings { ColorString = "White", Type = LightSettings.MyLightType.Directional, Direction = direction }.ToDirectXLight(); ValidateWithDevice(l); }
public static Point3D[] GetDirection(Box box) { Matrix m = box.Location.GetMatrix(); var vectors = new Point3D[3]; vectors[0] = new Point3D(m[0, 0], m[1, 0], m[2, 0]); vectors[1] = new Point3D(m[0, 1], m[1, 1], m[2, 1]); vectors[2] = new Point3D(m[0, 2], m[1, 2], m[2, 2]); //var up = vectors[2]; //var left = -vectors[1]; //var front = vectors[0]; return vectors; }
public void Point() { var position = new Point3D(200, 300, -100); Light l = new LightSettings { ColorString = "White", Type = LightSettings.MyLightType.Point, Position = position, }.ToDirectXLight(); Assert.AreEqual(position.ToDirectXVector(), l.Position); ValidateWithDevice(l); }
public void CreateBeachHut(string id, Point2D location, SideColor color) { var hut = GameObject.CreatePrimitive(PrimitiveType.Cube); Point3D size = new Point3D(12, 12, 16); hut.transform.position = new Vector3((float)location.X, (float)size.Z / 2, (float)location.Y); hut.transform.localScale = new Vector3((float)size.X, (float)size.Z, (float)size.Y); hut.GetComponent<Renderer>().material.color = GetDrawingColor(color); hut.name = id; }
public void Spot() { var position = new Point3D(200, 300, -100); var direction = new Point3D(0, 0, -1); Light l = new LightSettings { ColorString = "White", Type = LightSettings.MyLightType.Spot, Position = position, Direction = direction }.ToDirectXLight(); ValidateWithDevice(l); }
//Проецирование точек на соответствующую плоскость private static Point2D ProjectPoint(int i, Point3D p) //i - плоскость проектирования { switch (i) { case 0: return new Point2D(p.Y, p.Z); //0 - проецирование на yOz case 1: return new Point2D(p.X, p.Z); //1 - проецирование на xOz case 2: return new Point2D(p.X, p.Y); //2 - проецирование на xOy default: throw new Exception("Invalid plane for projection"); } }
DWMTestEntry GAXTest(Point3D accelerations, Angle AroundX, Angle AroundY, Angle AroundZ, params DWMCommand[] command) { return (client, world, asserter) => { DWMSensorsData data = new DWMSensorsData(); foreach (var c in command) data = client.Act(c); asserter.IsEqual(accelerations.X, data.GAX.Last().Accelerations.X, 0); asserter.IsEqual(accelerations.Y, data.GAX.Last().Accelerations.Y, 0); asserter.IsEqual(accelerations.Z, data.GAX.Last().Accelerations.Z, 0); asserter.IsEqual(AroundZ.Radian, data.GAX.Last().VelocityAroundZ.Radian, 0); }; }
/// <summary> /// Возвращает точку пересечения луча и круга параллельного плоскости z = 0 /// </summary> /// <param name="center">Вершина</param> /// <param name="radius">Радиус</param> /// <param name="ray">Луч</param> /// <returns></returns> public static Point3D? IntersectCircle(Point3D center, double radius, Ray ray) { var p1 = new Point3D(center.X - radius, center.Y, center.Z); var p2 = new Point3D(center.X, center.Y - radius, center.Z); var plane = new ePlane(center, p1, p2); Point3D? point1 = IntersectPlane(plane, ray); if ((point1 != null) && (center.Hypot(point1.Value) <= radius)) { return point1; } else { return null; } }
/// <summary> /// Возвращает точку пересечения луча и треугольника p0p1p2 /// </summary> /// <param name="p0">Вершина</param> /// <param name="p1">Вершина</param> /// <param name="p2">Вершина</param> /// <param name="ray">Луч</param> /// <returns></returns> public static Point3D? IntersectTriangle(Point3D p0, Point3D p1, Point3D p2, Ray ray) { var plane = new ePlane(p0, p1, p2); //Плоскость содержащая треугольник Point3D? point1 = IntersectPlane(plane, ray); if (point1 == null) { return null; } else { int i = ProjectPlane(plane); //Определяем куда проецировать треугольник Point2D point = ProjectPoint(i, point1.Value); Point2D v0 = ProjectPoint(i, p0); Point2D v1 = ProjectPoint(i, p1); Point2D v2 = ProjectPoint(i, p2); Point2D[] regionPoints = new Point2D[] {v0, v2, v1}; return Geometry.IsFromRegion(point, regionPoints) ? point1 : null; } }
/* * There are some not obvious calculations. * With them we can find the closest points of * two straits. Function returns mean point of * them. * p11, p21 - some points of straits. * p12, p22 - some other points of straits. * a1, a2 - vectors of straits. */ public static Line3D GetPerpendicular(Line3D line1, Line3D line2) { Point3D p11 = line1.Begin; Point3D p21 = line2.Begin; Point3D a1 = line1.Direction.Normalize(); Point3D a2 = line2.Direction.Normalize(); if (Math.Abs(a1.X - a2.X) < Epsilon && Math.Abs(a1.Y - a2.Y) < Epsilon && Math.Abs(a1.Z - a2.Z) < Epsilon) throw new Exception("straits are parallel"); var p12 = new Point3D(p11.X + a1.X, p11.Y + a1.Y, p11.Z + a1.Z); var p22 = new Point3D(p21.X + a2.X, p21.Y + a2.Y, p21.Z + a2.Z); double p1 = (p12.X - p11.X)*(p12.X - p11.X) + (p12.Y - p11.Y)*(p12.Y - p11.Y) + (p12.Z - p11.Z)*(p12.Z - p11.Z); double p2 = (p12.X - p11.X)*(p22.X - p21.X) + (p12.Y - p11.Y)*(p22.Y - p21.Y) + (p12.Z - p11.Z)*(p22.Z - p21.Z); double q1 = -((p22.X - p21.X)*(p12.X - p11.X) + (p22.Y - p21.Y)*(p12.Y - p11.Y) + (p22.Z - p21.Z)*(p12.Z - p11.Z)); double q2 = -((p22.X - p21.X)*(p22.X - p21.X) + (p22.Y - p21.Y)*(p22.Y - p21.Y) + (p22.Z - p21.Z)*(p22.Z - p21.Z)); double r1 = (p21.X - p11.X)*(p12.X - p11.X) + (p21.Y - p11.Y)*(p12.Y - p11.Y) + (p21.Z - p11.Z)*(p12.Z - p11.Z); double r2 = (p21.X - p11.X)*(p22.X - p21.X) + (p21.Y - p11.Y)*(p22.Y - p21.Y) + (p21.Z - p11.Z)*(p22.Z - p21.Z); double m = (q2*r1 - q1*r2)/(p1*q2 - p2*q1); double n = (p1*r2 - p2*r1)/(p1*q2 - p2*q1); double x1 = p11.X + m*(p12.X - p11.X); double y1 = p11.Y + m*(p12.Y - p11.Y); double z1 = p11.Z + m*(p12.Z - p11.Z); double x2 = p21.X + n*(p22.X - p21.X); double y2 = p21.Y + n*(p22.Y - p21.Y); double z2 = p21.Z + n*(p22.Z - p21.Z); return new Line3D(new Point3D(x1, y1, z1), new Point3D(x2, y2, z2)); }
public static double IntersectBall(Ball ball, Ray ray) { //Переменные для возврата корней квадратного уравнения double root1, root2; //Расчет коэффициентов решения системы уравнений сферы и луча var a = ray.Direction.MultiplyScalar(ray.Direction); //Вспомогательный вектор var location = new Point3D(ball.GetAbsoluteLocation().X, ball.GetAbsoluteLocation().Y, ball.GetAbsoluteLocation().Z + ball.Radius); var vec = ray.Origin - location; var b = 2 * ray.Direction.MultiplyScalar(vec); var c = vec.MultiplyScalar(vec) - Math.Pow(ball.Radius, 2); //Обработка решения if ((QuadEquation.Solution(a, b, c, out root1, out root2)) && ((root1 >= 0) | (root2 >= 0))) if (root1 >= 0) { return ray.DistanceTo(root1); } else { return ray.DistanceTo(root2); } return Double.PositiveInfinity; }
public static Point3D GetFrontDirection(Frame3D frame) { Matrix m = frame.GetMatrix(); var front = new Point3D(m[0, 0], m[1, 0], m[2, 0]); return front; }
public void InitSandObject(GameObject obj, string id, Point3D location) { obj.name = id; obj.transform.position = new Vector3((float)location.X, (float)location.Z, (float)location.Y); obj.GetComponent<Renderer>().material.color = Color.yellow; obj.AddComponent<Rigidbody>(); obj.GetComponent<Rigidbody>().drag = obj.GetComponent<Rigidbody>().angularDrag = 4; obj.GetComponent<Rigidbody>().useGravity = true; obj.GetComponent<Rigidbody>().mass = 0.3f; obj.GetComponent<Rigidbody>().constraints = RigidbodyConstraints.FreezeRotationX | RigidbodyConstraints.FreezeRotationZ; }
public void CreateSandCylinder(string id, Point3D location) { var cylinder = GameObject.CreatePrimitive(PrimitiveType.Cylinder); cylinder.transform.localScale = new Vector3(5.8f, 5.8f / 2, 5.8f); InitSandObject(cylinder, id, location); }
public void CreateSandCube(string id, Point3D location) { var cube = GameObject.CreatePrimitive(PrimitiveType.Cube); cube.transform.localScale = new Vector3(5.8f, 5.8f, 5.8f); InitSandObject(cube, id, location); }
public void CreateSandCone(string id, Point3D location) { var cone = GameObject.Instantiate(conePrefab); cone.transform.localScale = new Vector3(5.8f / 2, 5.8f / 2, 5.8f / 2); InitSandObject(cone, id, location); }
public void RayTest(Point3D v0, Point3D v1, Point3D v2, Ray ray, Point3D? point) { var computedPoint = Intersector.TriangleIntersection.IntersectTriangle(v0, v1, v2, ray); Assert.AreEqual(point, computedPoint); // Console.WriteLine("Expected: {0}. But was: {1}", point, computedPoint); }
public void RayTest(Ray ray, Point3D center, double radius, Point3D? point) { Point3D? computedPoint = Intersector.IntersectCircle(center, radius, ray); Assert.AreEqual(point, computedPoint); { //Console.WriteLine("Point: {0}. Expected: {1}", computedPoint, point); } }
public Point3D Apply(Point3D arg) { Frame3D r = Apply(arg.ToFrame()); return(new Point3D(r.X, r.Y, r.Z)); }
public Frame3D NewPoint(Point3D newPoint) { return(new Frame3D(newPoint.X, newPoint.Y, newPoint.Z, Pitch, Yaw, Roll)); }
public static double TripleProduct(Point3D a, Point3D b, Point3D c) { return a.X*(b.Y*c.Z - b.Z*c.Y) - a.Y*(b.X*c.Z - b.Z*c.X) + a.Z*(b.X*c.Y - b.Y*c.X); }
public Point3D ProjectionOnAxis(Point3D axis) { if (axis.Norm() < double.Epsilon) throw new Exception("Axis length shouldn't be 0"); return axis*axis.MultiplyScalar(this)/Math.Pow(axis.Norm(), 2); }
public void VerticalTest(Frame3D frame, Angle angle, Point3D rotated) { Point3D computed = SensorRotation.VerticalRotation(frame, angle); Assert.AreEqual(rotated, computed); Console.WriteLine("Expected: {0}. But was: {1}", rotated, computed); }