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);
}