public void PlaneIntersection_PlaneInOriginOfRay()
{
var ray = new Ray3D(new Point3D(0, 0, 0), new Vector3D(1, 2, 3));
Point3D p;
Assert.IsTrue(ray.PlaneIntersection(ray.Origin, ray.Direction, out p));
Assert.AreEqual(0, ray.Origin.DistanceTo(p), 1e-12);
}
public void GetNearest_PointOnRay()
{
var ray = new Ray3D(new Point3D(0, 0, 0), new Vector3D(1, 2, 3));
var p0 = new Point3D(0.1, 0.2, 0.3);
var p = ray.GetNearest(p0);
Assert.AreEqual(0, p0.DistanceTo(p), 1e-12);
}
public void RayInterSection_OutsideSpherePointingAway_NoIntersection()
{
var sphere = new BoundingSphere(new Point3D(0.2, 0.3, 0), 1.0);
var ray = new Ray3D(sphere.Center + new Vector3D(0.2, 0.3, sphere.Radius), new Vector3D(1, 0.1, 0.1));
Point3D[] result;
Assert.IsFalse(sphere.RayIntersection(ray, out result));
}
public void PlaneIntersection_RayThroughPlaneOrigin()
{
var ray = new Ray3D(new Point3D(1, 2, 3), new Vector3D(-1, -2, -3));
Point3D p;
Assert.IsTrue(ray.PlaneIntersection(new Point3D(0, 0, 0), new Vector3D(1, 1, 1), out p));
var pe = new Point3D(0, 0, 0);
Assert.AreEqual(0, pe.DistanceTo(p), 1e-12);
}
public void LineToTest()
{
var ray = new Ray3D(new Point3D(0, 0, 0), UnitVector3D.ZAxis);
var point3D = new Point3D(1, 0, 0);
Line3D line3DTo = ray.LineTo(point3D);
AssertGeometry.AreEqual(new Point3D(0, 0, 0), line3DTo.StartPoint);
AssertGeometry.AreEqual(point3D, line3DTo.EndPoint, float.Epsilon);
}
public void RayInterSection_OnSpherePointingOut_OneIntersection()
{
var sphere = new BoundingSphere(new Point3D(0.2, 0.3, 0), 2.0);
var ray = new Ray3D(sphere.Center + new Vector3D(0, 0, sphere.Radius), new Vector3D(1, 0.99, 0.87));
Point3D[] result;
Assert.IsTrue(sphere.RayIntersection(ray, out result));
Assert.AreEqual(1, result.Length, "Number of intersections should be 1");
Assert.AreEqual(sphere.Radius, sphere.Center.DistanceTo(result[0]), 1e-6, "Point is not on sphere.");
Assert.AreEqual(0, ray.GetNearest(result[0]).DistanceTo(result[0]), 1e-6, "Point is not on ray.");
}
public void PlaneIntersection_RayThroughPlaneOrigin()
{
var ray = new Ray3D(new Point3D(1, 2, 3), new Vector3D(-1, -2, -3));
Point3D p;
Assert.IsTrue(ray.PlaneIntersection(new Point3D(0, 0, 0), new Vector3D(1, 1, 1), out p));
var pe = new Point3D(0, 0, 0);
Assert.AreEqual(0, pe.DistanceTo(p), 1e-12);
}
public void RayInterSection_OnSpherePointingOut_OneIntersection()
{
var sphere = new BoundingSphere(new Point3D(0.2, 0.3, 0), 2.0);
var ray = new Ray3D(sphere.Center + new Vector3D(0, 0, sphere.Radius), new Vector3D(1, 0.99, 0.87));
Point3D[] result;
Assert.IsTrue(sphere.RayIntersection(ray, out result));
Assert.AreEqual(1, result.Length, "Number of intersections should be 1");
Assert.AreEqual(sphere.Radius, sphere.Center.DistanceTo(result[0]), 1e-6, "Point is not on sphere.");
Assert.AreEqual(0, ray.GetNearest(result[0]).DistanceTo(result[0]), 1e-6, "Point is not on ray.");
}
public double Intersect(Ray3D ray)
{
var d = this.Plane().Intersect(ray);
var bary = this.ToBarycentric(ray.PointAt(d));
if (bary.X < 0.0 || bary.Y < 0.0 || bary.Z < 0.0)
{
return(double.PositiveInfinity);
}
return(d);
}
private BoundingVolume BuildFrustumFromViewport(Viewport3DHitTestHelper.ViewportInformation viewport, List <Point> frustumOutline)
{
List <Ray3D> list1 = new List <Ray3D>(frustumOutline.Count);
Point point1 = new Point(0.0, 0.0);
for (int index = 0; index < frustumOutline.Count; ++index)
{
list1.Add(CameraRayHelpers.RayFromViewportPoint(viewport.Bounds, viewport.Camera, frustumOutline[index]));
point1 += (Vector)frustumOutline[index];
}
Point point2 = (Point)Vector.Divide((Vector)point1, (double)frustumOutline.Count);
Point3D point3D = new Point3D(0.0, 0.0, 0.0);
List <Plane3D> list2 = new List <Plane3D>();
for (int index = 0; index < frustumOutline.Count; ++index)
{
Vector3D normal = Vector3D.AngleBetween(list1[index].Direction, list1[(index + 1) % frustumOutline.Count].Direction) <= 0.0001 ? Vector3D.CrossProduct(list1[index].Direction, list1[(index + 1) % frustumOutline.Count].Origin - list1[index].Origin) : Vector3D.CrossProduct(list1[index].Direction, list1[(index + 1) % frustumOutline.Count].Direction);
normal.Normalize();
list2.Add(new Plane3D(normal, list1[index].Origin));
point3D += (Vector3D)list1[index].Origin;
}
bool flag = list2[0].GetSignedDistanceFromPoint(list1[2].Origin) < 0.0;
BoundingVolume boundingVolume = new BoundingVolume();
for (int index = 0; index < frustumOutline.Count; ++index)
{
Plane3D frustumSide = list2[index];
if (flag)
{
Vector3D normal = frustumSide.Normal;
normal.Negate();
frustumSide = new Plane3D(normal, list1[index].Origin);
}
boundingVolume.AddSide(frustumSide);
}
Ray3D ray3D = CameraRayHelpers.RayFromViewportPoint(viewport.Bounds, viewport.Camera, point2);
this.frustumCenterRay = new Ray3D((Point3D)Vector3D.Divide((Vector3D)point3D, (double)frustumOutline.Count), ray3D.Direction);
ProjectionCamera projectionCamera = viewport.Camera as ProjectionCamera;
if (projectionCamera != null)
{
boundingVolume.NearFrustum = new Plane3D(ray3D.Direction, this.frustumCenterRay.Origin);
if (!double.IsPositiveInfinity(projectionCamera.FarPlaneDistance))
{
Vector3D direction = ray3D.Direction;
direction.Negate();
boundingVolume.FarFrustum = new Plane3D(direction, this.frustumCenterRay.Origin + (projectionCamera.FarPlaneDistance - projectionCamera.NearPlaneDistance) * ray3D.Direction);
}
}
return(boundingVolume);
}
public override Task <Tuple <bool, double> > CheckPanelIntersectionAsync(bool targetLinkState, Vector3D linkDirection)
{
if (targetLinkState)
{
var ptPanel = ColliderExtensions.GetPanel();
if (ptPanel.HasValue)
{
if (Points.Count > 0)
{
//var zDir = new Vector3D(0.0, 0.0, 1.0);
var zDir = GetPanelFaceDirectionForLinkImpact(linkDirection);
var panel = ptPanel.Value;
var tt = TotalTransformation.Value;
//var planePoint = panel.Location + zDir * panel.SizeZ;
var panelSize = GetPanelSizeForLinkImpact(panel, linkDirection);
var planePoint = panel.Location + zDir * panelSize;
var tasks = new List <Task <Tuple <bool, double> > >();
foreach (var p in Points)
{
var point = p;
tasks.Add(Task.Run(() =>
{
var pp = tt.Transform(point);
var ray = new Ray3D(pp, linkDirection);
var pi = ray.PlaneIntersection(planePoint, zDir);
if (pi.HasValue && panel.Contains(pi.Value))
{
var intersectValue = Vector3D.DotProduct(pi.Value - pp, linkDirection);
return(new Tuple <bool, double>(true, intersectValue));
}
else
{
return(new Tuple <bool, double>(false, 0.0));
}
}));
}
return(Task.WhenAll(tasks).ContinueWith((t) => t.Result.FirstOrDefault(r => r.Item1) ?? new Tuple <bool, double>(false, 0.0)));
}
else
{
throw new ArgumentOutOfRangeException("The number of points of collider must be greater than 0!");
}
}
}
return(Task.FromResult(new Tuple <bool, double>(false, 0.0)));
}
public void BinaryRountrip()
{
var v = new Ray3D(new Point3D(1, 2, -3), new UnitVector3D(1, 2, 3));
using (var ms = new MemoryStream())
{
var formatter = new BinaryFormatter();
formatter.Serialize(ms, v);
ms.Flush();
ms.Position = 0;
var roundTrip = (Ray3D)formatter.Deserialize(ms);
AssertGeometry.AreEqual(v, roundTrip);
}
}
public RayDifferential3D(Point3D origin, Vector3D direction, float start = EPSILON, float end = float.MaxValue, float time = 0.0f)
{
this.Origin = origin;
this.Direction = direction;
MinT = start;
MaxT = end;
Time = time;
HasDifferentials = false;
RayX = new Ray3D();
RayY = new Ray3D();
}
public RayDifferential3D(Point3D origin, Vector3D direction, float start = EPSILON, float end = float.MaxValue, float time = 0.0f)
{
this.Origin = origin;
this.Direction = direction;
MinT = start;
MaxT = end;
Time = time;
HasDifferentials = false;
RayX = new Ray3D();
RayY = new Ray3D();
}
private Point3D?GetNearestPoint(Point position, Point3D hitPlaneOrigin, Vector3D hitPlaneNormal)
{
var hpp = this.GetHitPlanePoint(position, hitPlaneOrigin, hitPlaneNormal);
if (hpp == null)
{
return(null);
}
var ray = new Ray3D(this.ToWorld(this.Position), this.ToWorld(this.Direction));
return(ray.GetNearest(hpp.Value));
}
public static Vector3D VectorBetweenPointsOnPlane(Viewport3D viewport, Plane3D plane, Point initialPoint, Point endPoint)
{
Size viewportSize = new Size(viewport.ActualWidth, viewport.ActualHeight);
Ray3D ray1 = CameraRayHelpers.RayFromViewportPoint(viewportSize, viewport.Camera, initialPoint);
Ray3D ray2 = CameraRayHelpers.RayFromViewportPoint(viewportSize, viewport.Camera, endPoint);
double t1;
double t2;
if (!plane.IntersectWithRay(ray1, out t1) || !plane.IntersectWithRay(ray2, out t2))
{
return(new Vector3D(0.0, 0.0, 0.0));
}
return(ray2.Evaluate(t2) - ray1.Evaluate(t1));
}
public void RayInterSection_OutsideSpherePointingToCenter_TwoIntersections()
{
var sphere = new BoundingSphere(new Point3D(0.2, 0.3, 0), 1.0);
var p = new Point3D(12, 23, 32);
var ray = new Ray3D(p, sphere.Center - p);
Point3D[] result;
Assert.IsTrue(sphere.RayIntersection(ray, out result));
Assert.AreEqual(sphere.Radius, sphere.Center.DistanceTo(result[0]), 1e-6, "Point 1 is not on sphere.");
Assert.AreEqual(0, ray.GetNearest(result[0]).DistanceTo(result[0]), 1e-6, "Point 1" + result[0] + " is not on ray.");
Assert.AreEqual(sphere.Radius, sphere.Center.DistanceTo(result[1]), 1e-6, "Point 2 is not on sphere.");
Assert.AreEqual(0, ray.GetNearest(result[1]).DistanceTo(result[1]), 1e-6, "Point 2 " + result[0] + " is not on ray.");
Assert.IsTrue(ray.Origin.DistanceTo(result[0]) < ray.Origin.DistanceTo(result[1]), "The points should be sorted by distance from ray origin.");
}
public void RayInterSection_OutsideAndTangentToSphere_OneIntersection()
{
var sphere = new BoundingSphere(new Point3D(0.2, 0.3, 0), 1.0);
var p = new Point3D(sphere.Center.X + sphere.Radius, sphere.Center.Y + sphere.Radius, sphere.Center.Z + 50);
var p2 = new Point3D(sphere.Center.X + sphere.Radius, sphere.Center.Y, sphere.Center.Z);
// ray tangent to sphere
var ray = new Ray3D(p, p2 - p);
Point3D[] result;
Assert.IsTrue(sphere.RayIntersection(ray, out result), "No intersection");
Assert.AreEqual(1, result.Length, "One intersection");
Assert.AreEqual(sphere.Radius, result[0].DistanceTo(sphere.Center), 1e-8);
Assert.AreEqual(0, ray.GetNearest(result[0]).DistanceTo(result[0]), 1e-6, "Point 1 is not on ray.");
}
private static bool ComputeLineTriangleIntersection(Ray3D line, Point3D[] triangle, out Point hitBarycentric)
{
hitBarycentric = new Point(double.NaN, double.NaN);
Vector3D vector3D1 = triangle[1] - triangle[0];
Vector3D vector3D2 = triangle[2] - triangle[0];
if (Tolerances.NearZero(Vector3D.CrossProduct(vector3D1, vector3D2).LengthSquared))
{
return(false);
}
Vector3D vector3D3 = line.Origin - triangle[0];
Vector3D vector2_1 = Vector3D.CrossProduct(line.Direction, vector3D2);
double d = Vector3D.DotProduct(vector3D1, vector2_1);
if (Tolerances.NearZero(d))
{
return(false);
}
Vector3D vector1;
if (d > 0.0)
{
vector1 = line.Origin - triangle[0];
}
else
{
vector1 = triangle[0] - line.Origin;
d = -d;
}
double num1 = 1.0 / d;
double num2 = Vector3D.DotProduct(vector1, vector2_1);
if (num2 < 0.0 || d < num2)
{
return(false);
}
Vector3D vector2_2 = Vector3D.CrossProduct(vector1, vector3D1);
double num3 = Vector3D.DotProduct(line.Direction, vector2_2);
if (num3 < 0.0 || d < num2 + num3)
{
return(false);
}
double num4 = Vector3D.DotProduct(vector3D2, vector2_2) * num1;
double x = num2 * num1;
double y = num3 * num1;
hitBarycentric = new Point(x, y);
return(true);
}
public void BinaryRountrip()
{
var v = new Ray3D(new Point3D(1, 2, -3), new UnitVector3D(1, 2, 3));
using (var ms = new MemoryStream())
{
var formatter = new BinaryFormatter();
formatter.Serialize(ms, v);
ms.Flush();
ms.Position = 0;
var roundTrip = (Ray3D)formatter.Deserialize(ms);
AssertGeometry.AreEqual(v, roundTrip);
}
}
public void RayInterSection_OutsideSpherePointingToCenter_TwoIntersections()
{
var sphere = new BoundingSphere(new Point3D(0.2, 0.3, 0), 1.0);
var p = new Point3D(12, 23, 32);
var ray = new Ray3D(p, sphere.Center - p);
Point3D[] result;
Assert.IsTrue(sphere.RayIntersection(ray, out result));
Assert.AreEqual(sphere.Radius, sphere.Center.DistanceTo(result[0]), 1e-6, "Point 1 is not on sphere.");
Assert.AreEqual(0, ray.GetNearest(result[0]).DistanceTo(result[0]), 1e-6, "Point 1" + result[0] + " is not on ray.");
Assert.AreEqual(sphere.Radius, sphere.Center.DistanceTo(result[1]), 1e-6, "Point 2 is not on sphere.");
Assert.AreEqual(0, ray.GetNearest(result[1]).DistanceTo(result[1]), 1e-6, "Point 2 " + result[0] + " is not on ray.");
Assert.IsTrue(ray.Origin.DistanceTo(result[0]) < ray.Origin.DistanceTo(result[1]), "The points should be sorted by distance from ray origin.");
}
public void RayInterSection_OutsideAndTangentToSphere_OneIntersection()
{
var sphere = new BoundingSphere(new Point3D(0.2, 0.3, 0), 1.0);
var p = new Point3D(sphere.Center.X + sphere.Radius, sphere.Center.Y + sphere.Radius, sphere.Center.Z + 50);
var p2 = new Point3D(sphere.Center.X + sphere.Radius, sphere.Center.Y, sphere.Center.Z);
// ray tangent to sphere
var ray = new Ray3D(p, p2 - p);
Point3D[] result;
Assert.IsTrue(sphere.RayIntersection(ray, out result), "No intersection");
Assert.AreEqual(1, result.Length, "One intersection");
Assert.AreEqual(sphere.Radius, result[0].DistanceTo(sphere.Center), 1e-8);
Assert.AreEqual(0, ray.GetNearest(result[0]).DistanceTo(result[0]), 1e-6, "Point 1 is not on ray.");
}
/// <summary>
/// Only if deep is true, then HitCubieResult could have some value
/// </summary>
/// <param name="pt"></param>
/// <param name="deep"></param>
/// <param name="hitResult"></param>
/// <returns></returns>
public bool HitTest(Point pt, bool deep, out HitResult hitResult)
{
hitResult = null;
Ray3D ray = Ext3D.Unproject(pt, _viewpoint, _model.Transform, _inverseViewMatrix, _inverseProjectionMatrix);
//Debug.WriteLine(string.Format("ray: {0}", ray));
double?d = this.BoundingBox.Intersects(ray);
if (!deep)
{
if (d != null)
{
//Debug.WriteLine(string.Format("first ray: {0}, distance:{1}", ray, (double)d));
hitResult = new HitResult()
{
Distance = (double)d, HitCubicle = null, HitPoint = ray.Origin + (double)d * ray.Direction
};
}
}
else
{
List <HitResult> results = new List <HitResult>();
if (d != null)
{
double?d1;
foreach (Cubicle cubicle in _cubicles.Values)
{
Matrix3D localToWorld = _model.Transform; localToWorld.Invert();// *cubicle.Cubie.Transform;
ray = Ext3D.Unproject(pt, _viewpoint, localToWorld, _inverseViewMatrix, _inverseProjectionMatrix);
//d1 = cubicle.Cubie.BoundingBox.Intersects(ray);
d1 = cubicle.BoundingBox.Intersects(ray);
if (d1 != null)
{
HitResult result = new HitResult()
{
Distance = (double)d1, HitCubicle = cubicle, HitPoint = ray.Origin + (double)d1 * ray.Direction
};
results.Add(result);
}
}
results.Sort((x, y) => x.Distance.CompareTo(y.Distance));
if (results.Count > 0)
{
hitResult = results[0];
}
}
}
return(d != null);
}
public bool DoesIntersect(Ray3D ray, List <Vector3> polyPoints, out double entry, out double depth)
{
double aMin;
double aMax;
GetAminAmax(ray, polyPoints, out aMin, out aMax);
List <double> cKs = GetCks(ray, polyPoints);
List <double> intersections = FindIntersectionParameters(ray, cKs, polyPoints);
float rayLength = ray.Source.DistanceTo(ray.Destination);
depth = GetRayDepth(rayLength, intersections, aMin, aMax);
entry = 0;
return(true);
}
public WallProjectionSample calculateSample(Point3D[] vectors, String label)
{
Ray3D ray = new Ray3D((Point3D)vectors[0], (Point3D)vectors[1]);
Point3D samplePoint = intersectAndTestAllWalls(ray);
WallProjectionSample sample = new WallProjectionSample(new Point3D(), "nullSample");
if ((samplePoint.X.Equals(float.NaN) == false))
{
if (label.Equals(""))
{
sample = new WallProjectionSample(samplePoint);
}
else
{
sample = new WallProjectionSample(samplePoint, label);
}
return(sample);
}
return(sample);
}
public Point3D getDeviceLocation(List <Point3D[]> positions)
{
//set new Position
if (positions.Count < MIN_NUMBER_OF_VECTORS) // cancel if not enough vectors in list
{
return(new Point3D(Double.NaN, Double.NaN, Double.NaN));
}
List <Ray3D> lines = new List <Ray3D>();
foreach (Point3D[] p in positions)
{
// pointing ray goes from p[0] to p[1]
Ray3D line = new Ray3D(p[0], p[1]);
lines.Add(line);
}
//Line3D.updateWeight(lines);
return(Locator.cobylaCentralPoint(lines.ToArray()));
}
public static bool FindIntersection(Ray3D ray, Triangle3D t, out bool isOnEdge)
{
double u, v, distance = 0;
Vector3 d = ray.Destination;
Vector3 T = ray.Source - t.P1;
if (T == null)
{
throw new ArgumentNullException("T");
}
Vector3 edge1 = t.P2 - t.P1;
Vector3 edge2 = t.P3 - t.P1;
Vector3 P = d.CrossMultiply(edge2);
Vector3 Q = T.CrossMultiply(edge1);
float row1, row2, row3;
row1 = (float)(Q * edge2);
row2 = (float)(P * T);
row3 = (float)(Q * d);
Vector3 result = new Vector3(row1, row2, row3) * (1.0f / (P * edge1));
distance = result[0];
u = result[1];
v = result[2];
bool test = u == 0;
bool test2 = v == 0;
isOnEdge = test || test2;
return(u >= 0 &&
v >= 0 &&
distance >= 0 &&
u + v <= 1);
}
public override void InitializeAgent()
{
if (usePlayerBrain)
{
MLAgents.PlayerBrain playerBrain = (MLAgents.PlayerBrain)Resources.Load("_RobotPlayerBrain");
this.brain = playerBrain;
print("Brain loaded");
}
rbMainBody = MainBody.GetComponent <Rigidbody>();
rbPalette = Palette.GetComponent <Rigidbody>();
rayPer = MainBody.GetComponent <Ray3D>();
forkPosition = forkMin;
maxAngularVelocity = Ray.DegreeToRadian(360) * rpm / 60;
rbMainBody.maxAngularVelocity = maxAngularVelocity;
GameObject ground = GameObject.Find("Ground");
var transform = ground.GetComponent <Transform>();
maxPositionValue = transform.lossyScale.x * 10 / 2;
minPositionValue = -maxPositionValue;
}
/// <summary>
/// Create a Ray3D from the Perspective Camera and the X- and Y Values of the MousePosition.
/// </summary>
/// <param name="camera">The PerspectiveCamera.</param>
/// <param name="xPosFromCenter">The X Position of the Mouse in the Range [-maxXValue, maxXValue].</param>
/// <param name="yPosFromCenter">The Y Position of the Mouse in the Range [-maxYValue, maxYValue].</param>
/// <returns>Ray3D originating in the Camera's Position, pointing to the MousePosition in 3D Space.</returns>
public static Ray3D GetRay3D(this PerspectiveCamera camera, double xPosFromCenter, double yPosFromCenter)
{
var ray = new Ray3D();
ray.Origin = camera.Position;
var forward = camera.LookDirection;
forward.Normalize();
var up = camera.UpDirection;
up.Normalize();
var left = Vector3D.CrossProduct(up, forward);
left.Normalize();
up = Vector3D.CrossProduct(camera.LookDirection, left);
up.Normalize();
var rayDirection = forward + -left * xPosFromCenter + up * yPosFromCenter;
rayDirection.Normalize();
ray.Direction = rayDirection;
return(ray);
}
private static bool AxisTest(Vector3 point, IList <Triangle3D> mesh, Vector3 axis)
{
var r = new Ray3D(new Vector3(point.X, point.Y, point.Z), axis);
double totalCount = 0;
foreach (Triangle3D t in mesh)
{
bool isOnEdge;
bool isColl = FindIntersection(r, t, out isOnEdge);
if (isColl)
{
if (isOnEdge)
{
totalCount += 0.5;
}
else
{
totalCount++;
}
}
}
return(totalCount % 2 != 0); //Is Odd
}
/// <summary>
/// Computes the intersection point between a 3D ray and this box.
/// </summary>
/// <param name="ray3D">The 3D ray.</param>
/// <returns>The intersection point, if one exists.</returns>
public Point3D?IntersectionWith(Ray3D ray3D)
{
// If the box is degenerate, return.
// TODO: Better handle degenerate (e.g. planar) cases
if (this.IsDegenerate)
{
return(null);
}
var results = new List <Point3D>();
// Compute the intersection points with each face of the box
foreach (Box3DFacet facet in Enum.GetValues(typeof(Box3DFacet)))
{
var point3D = this.GetFacet(facet).IntersectionWith(ray3D);
if (point3D.HasValue)
{
results.Add(point3D.Value);
}
}
// If there are any intersection points, get the closest one
return(results.Any() ? results.OrderBy(p => p.DistanceTo(ray3D.ThroughPoint)).First() : null);
}
public IEnumerable <HitInfo <V> > Raycast(RayDescription ray)
{
List <HitInfo <V> > hits = new List <HitInfo <V> >();
if (mesh.Topology != Topology.Triangles)
{
return(hits);
}
Ray3D r = new Ray3D(ray.Origin, ray.Direction);
for (int i = 0; i < mesh.Indices.Length / 3; i++)
{
V v1 = mesh.Vertices[mesh.Indices[i * 3 + 0]];
V v2 = mesh.Vertices[mesh.Indices[i * 3 + 1]];
V v3 = mesh.Vertices[mesh.Indices[i * 3 + 2]];
Triangle3D tri = new Triangle3D(v1.Position, v2.Position, v3.Position);
float t;
float3 baricenter;
if (tri.Intersect(r, out t, out baricenter))
{
if (t >= ray.MinT && t < ray.MaxT)
{
hits.Add(new HitInfo <V>
{
T = t,
Attribute = v1.Mul(baricenter.x).Add(v2.Mul(baricenter.y)).Add(v3.Mul(baricenter.z))
});
}
}
}
hits.Sort((h1, h2) => h1.T.CompareTo(h2.T));
return(hits);
}
public RectangleHitTestResultTreeLeaf(RectangleHitTestResultTreeNode parent, DependencyObject objectHit, BoundingVolume transformedFrustum, Ray3D frustumCenterRay, Matrix3D transform)
{
this.parent = parent;
this.objectHit = objectHit;
this.transformedFrustum = transformedFrustum;
this.frustumCenterRay = frustumCenterRay;
this.compositeTransform = transform;
}
static void CalculateRivetPose3D(Point3D cameraCenter1, Point3D pointCam1_1, Point3D pointCam1_2, Point3D cameraCenter2, Point3D pointCam2_1, Point3D pointCam2_2)
{
//Plane planeCam1 = new Plane(new Point3D(cameraCenter1.X, cameraCenter1.Y, cameraCenter1.Z), new Point3D(pointCam1_1.X, pointCam1_1.Y, pointCam1_1.Z), new Point3D(pointCam1_2.X, pointCam1_2.Y, pointCam1_2.Z));
Plane planeCam2 = new Plane(new Point3D(cameraCenter2.X, cameraCenter2.Y, cameraCenter2.Z), new Point3D(pointCam2_1.X, pointCam2_1.Y, pointCam2_1.Z), new Point3D(pointCam2_2.X, pointCam2_2.Y, pointCam2_2.Z));
Vector3D VectorCam1CenterToPoint1_1 = new Vector3D(pointCam1_1.X - cameraCenter1.X, pointCam1_1.Y - cameraCenter1.Y, pointCam1_1.Z - cameraCenter1.Z);
Vector3D VectorCam1CenterToPoint1_2 = new Vector3D(pointCam1_2.X - cameraCenter1.X, pointCam1_2.Y - cameraCenter1.Y, pointCam1_2.Z - cameraCenter1.Z);
//Ray3D rivetAxis3D = planeCam1.IntersectionWith(planeCam2);
Ray3D RayCam1CenterToPoint1_1 = new Ray3D(cameraCenter1, VectorCam1CenterToPoint1_1.Normalize());
Ray3D RayCam1CenterToPoint1_2 = new Ray3D(cameraCenter1, VectorCam1CenterToPoint1_2.Normalize());
Point3D pointRivetCylinder1 = planeCam2.IntersectionWith(RayCam1CenterToPoint1_1);
Point3D pointRivetCylinder2 = planeCam2.IntersectionWith(RayCam1CenterToPoint1_2);
//var a = rivetAxis3D.LineTo(pointRivetCylinder1);
//var b = rivetAxis3D.LineTo(pointRivetCylinder2);
//Angle alpha1 = RayCam1CenterToPoint1_1.Direction.AngleTo(rivetAxis3D.Direction);
//Angle alpha2 = RayCam1CenterToPoint1_2.Direction.AngleTo(rivetAxis3D.Direction);
UnitVector3D intersection1To2 = new Vector3D(pointRivetCylinder2.X - pointRivetCylinder1.X, pointRivetCylinder2.Y - pointRivetCylinder1.Y, pointRivetCylinder2.Z - pointRivetCylinder1.Z).Normalize();
Angle alpha1 = RayCam1CenterToPoint1_1.Direction.AngleTo(intersection1To2);
Angle alpha2 = RayCam1CenterToPoint1_2.Direction.AngleTo(intersection1To2);
if (pointRivetCylinder1.DistanceTo(new Point3D(0, 0, 0)) < pointRivetCylinder2.DistanceTo(new Point3D(0, 0, 0)))
{
double distance1 = Math.Abs(_rivetRadius / Math.Tan(alpha1.Radians));
Vector3D distanceAlongRivetAxis1 = distance1 * intersection1To2;
_rivetCenter1 = pointRivetCylinder1 + distanceAlongRivetAxis1;
double distance2 = Math.Abs(_rivetRadius / Math.Tan(alpha2.Radians));
Vector3D distanceAlongRivetAxis2 = distance2 * intersection1To2;
_rivetCenter2 = pointRivetCylinder2 - distanceAlongRivetAxis2;
}
else
{
double distance1 = _rivetRadius / Math.Tan(alpha1.Radians);
Vector3D distanceAlongRivetAxis1 = distance1 * intersection1To2;
_rivetCenter1 = pointRivetCylinder1 - distanceAlongRivetAxis1;
double distance2 = _rivetRadius / Math.Tan(alpha2.Radians);
Vector3D distanceAlongRivetAxis2 = distance2 * intersection1To2;
_rivetCenter2 = pointRivetCylinder2 + distanceAlongRivetAxis2;
}
//MathNet.Spatial.Units.Angle plane1AxisAngle = planeCam1.Normal.AngleTo(rivetAxis3D.Direction);
double rivetLength = Math.Sqrt(Math.Pow(_rivetCenter1.X - _rivetCenter2.X, 2) + Math.Pow(_rivetCenter1.Y - _rivetCenter2.Y, 2) + Math.Pow(_rivetCenter1.Z - _rivetCenter2.Z, 2));
//Console.WriteLine("distance between points: " + rivetLength.ToString());
//var length1 = rivetAxis3D.LineTo(pointRivetCylinder1).Length;
//var length2 = rivetAxis3D.LineTo(pointRivetCylinder2).Length;
//var collinear = rivetAxis3D.Direction.IsParallelTo(intersection1To2);
}
/// <summary>
/// Gets the nearest point on the translation axis.
/// </summary>
/// <param name="position">
/// The position (in screen coordinates).
/// </param>
/// <param name="hitPlaneOrigin">
/// The hit plane origin (world coordinate system).
/// </param>
/// <param name="hitPlaneNormal">
/// The hit plane normal (world coordinate system).
/// </param>
/// <returns>
/// The nearest point (world coordinates) or null if no point could be found.
/// </returns>
private Point3D? GetNearestPoint(Point position, Point3D hitPlaneOrigin, Vector3D hitPlaneNormal)
{
var hpp = this.GetHitPlanePoint(position, hitPlaneOrigin, hitPlaneNormal);
if (hpp == null)
{
return null;
}
var ray = new Ray3D(this.ToWorld(this.Position), this.ToWorld(this.Direction));
return ray.GetNearest(hpp.Value);
}
public void PlaneIntersection_RayInPlane()
{
var ray = new Ray3D(new Point3D(0, 0, 0), new Vector3D(1, 0, 0));
Point3D p;
Assert.IsFalse(ray.PlaneIntersection(ray.Origin, ray.Direction.FindAnyPerpendicular(), out p));
}
public MeshGeometryRectangleHitTestResultTreeLeaf(RectangleHitTestResultTreeNode parent, DependencyObject objectHit, MeshGeometry3D geometryHit, BoundingVolume transformedFrustum, Ray3D frustumCenterRay, Matrix3D transform, Material frontMaterial, Material backMaterial)
: base(parent, objectHit, transformedFrustum, frustumCenterRay, transform)
{
this.geometryHit = geometryHit;
this.frontMaterial = frontMaterial;
this.backMaterial = backMaterial;
}
/// <summary>
/// Converts a 3D ray to a tab-delimited text representation.
/// </summary>
/// <param name="ray3D">The 3D ray.</param>
/// <returns>Tab-delimited text representation.</returns>
internal static string ToText(this Ray3D ray3D) =>
$"{ray3D.ThroughPoint.ToText()}\t{ray3D.Direction.ToVector3D().ToText()}";
public static void AreEqual(Ray3D expected, Ray3D actual, double tolerance = 1e-6, string message = "")
{
AreEqual(expected.ThroughPoint, actual.ThroughPoint, tolerance, message);
AreEqual(expected.Direction, actual.Direction, tolerance, message);
}
public static void AreEqual(Ray3D expected, Ray3D actual, double tolerance = 1e-6, string message = "")
{
AreEqual(expected.ThroughPoint, actual.ThroughPoint, tolerance, message);
AreEqual(expected.Direction, actual.Direction, tolerance, message);
}
public void Parse(GameBitBuffer buffer)
{
Field0 = new Ray3D();
Field0.Parse(buffer);
Field1 = buffer.ReadFloat32();
}
public double TestAngle(Point pt, HitResult prevHit, Axis axis)
{
Ray3D ray = Ext3D.Unproject(pt, _viewpoint, _model.Transform, _inverseViewMatrix, _inverseProjectionMatrix);
//Plane yz = new Plane(-1, 0, 0, prevHit.HitPoint.X);
//Point3 pyz; double? d = yz.Intersects(ray); Debug.WriteLine(string.Format("second ray: {0}: distance: {1}", ray, (double)d));
//Debug.WriteLine(string.Format("second ray: {0}", ray));
double angle = 0;
switch (axis)
{
case Axis.X:
Plane3D yz = new Plane3D(new Vector3D(-1, 0, 0), prevHit.HitPoint.X); Point3D oyz = new Point3D(prevHit.HitPoint.X, 0, 0);
Point3D pyz; yz.Intersect(ray, out pyz); Vector3D from = prevHit.HitPoint - oyz; Vector3D to = pyz - oyz;
angle = Ext3D.AngleBetween(from, to);
if (Vector3D.DotProduct(Ext3D.UnitX, Vector3D.CrossProduct(from, to)) < 0)
{
angle = -angle;
}
break;
case Axis.Z:
Plane3D xy = new Plane3D(new Vector3D(0, 0, -1), prevHit.HitPoint.Z); Point3D oxy = new Point3D(0, 0, prevHit.HitPoint.Z);
Point3D pxy; xy.Intersect(ray, out pxy); from = prevHit.HitPoint - oxy; to = pxy - oxy;
angle = Ext3D.AngleBetween(from, to);
if (Vector3D.DotProduct(Ext3D.UnitZ, Vector3D.CrossProduct(from, to)) < 0)
{
angle = -angle;
}
break;
case Axis.Y:
Plane3D zx = new Plane3D(new Vector3D(0, -1, 0), prevHit.HitPoint.Y); Point3D ozx = new Point3D(0, prevHit.HitPoint.Y, 0);
Point3D pzx; zx.Intersect(ray, out pzx); from = prevHit.HitPoint - ozx; to = pzx - ozx;
angle = Ext3D.AngleBetween(from, to);
if (Vector3D.DotProduct(Ext3D.UnitY, Vector3D.CrossProduct(from, to)) < 0)
{
angle = -angle;
}
break;
}
/*
* if (angle < 2)
* angle = null;
* else
* {
* Debug.WriteLine(string.Format("axis: {0}, angle:{1}", axis, angle));
* }
*/
return(angle);
}
public void XmlTests(string ps, string vs, bool asElements, string xml)
{
var ray = new Ray3D(Point3D.Parse(ps), UnitVector3D.Parse(vs));
AssertXml.XmlRoundTrips(ray, xml, (e, a) => AssertGeometry.AreEqual(e, a, 1e-6));
}
public IEnumerable <HitInfo <V> > Raycast(RayDescription ray)
{
if (mesh.Topology != Topology.Triangles)
{
yield break;
}
Ray3D r = new Ray3D(ray.Origin, ray.Direction + 0.000000001f); // epsilon deviation of the direction to avoid indefinitions
float minT, maxT;
if (!box.Intersect(r, out minT, out maxT))
{
yield break;
}
maxT = min(maxT, ray.MaxT);
float t = max(ray.MinT, minT);
float3 P = r.X + r.D * t;
int3 cell = (int3)min(((P - box.Minimum) * resolution / (box.Maximum - box.Minimum)), resolution - 1);
float3 side = r.D > 0;
int3 cellInc = (r.D > 0) * 2 - 1;
float3 corner = box.Minimum + (cell + side) * (box.Maximum - box.Minimum) / resolution;
float3 alphas = (corner - r.X) / r.D;
float3 alphaInc = (box.Maximum - box.Minimum) / abs(resolution * r.D);
while (t < maxT)
{
float nextT = min(alphas.x, min(alphas.y, alphas.z));
if (any(cell < 0) || any(cell >= resolution))
{
yield break; // just for numerical problems, traversal could go outside grid.
}
// check current cell
if (triangleHash[cell.z, cell.y, cell.x] != null)
{
List <HitInfo <V> > hits = new List <HitInfo <V> >();
foreach (var i in triangleHash[cell.z, cell.y, cell.x])
{
V v1 = mesh.Vertices[mesh.Indices[i * 3 + 0]];
V v2 = mesh.Vertices[mesh.Indices[i * 3 + 1]];
V v3 = mesh.Vertices[mesh.Indices[i * 3 + 2]];
Triangle3D tri = new Triangle3D(v1.Position, v2.Position, v3.Position);
float triT;
float3 baricenter;
if (tri.Intersect(r, out triT, out baricenter))
{
if (triT >= t && triT <= nextT)
{
hits.Add(new HitInfo <V>
{
T = triT,
Attribute = v1.Mul(baricenter.x).Add(v2.Mul(baricenter.y)).Add(v3.Mul(baricenter.z))
});
}
}
}
hits.Sort((h1, h2) => h1.T.CompareTo(h2.T)); // only need to sort hits inside a cell, because cells are iterated in ray-order
foreach (var hi in hits)
{
yield return(hi);
}
}
// advance ray to next cell
int3 movement = new int3(alphas.x <= alphas.y && alphas.x <= alphas.z, alphas.y < alphas.x && alphas.y <= alphas.z, alphas.z < alphas.x && alphas.z < alphas.y);
cell += movement * cellInc;
alphas += movement * alphaInc;
t = nextT;
}
}
public void SignedDistanceToRay(string prps, string pns, string rayThroughPointString, string rayDirectionString, double expectedValue)
{
var plane = new Plane(UnitVector3D.Parse(pns), Point3D.Parse(prps));
var otherPlane = new Ray3D(Point3D.Parse(rayThroughPointString), UnitVector3D.Parse(rayDirectionString));
Assert.AreEqual(expectedValue, plane.SignedDistanceTo(otherPlane), 1E-6);
}
/// <summary>
/// Gets the intersection with the specified ray.
/// </summary>
/// <param name="ray">The ray.</param>
/// <param name="result">The intersection point(s).</param>
/// <returns>The intersection points sorted by distance from the ray origin.</returns>
public bool RayIntersection(Ray3D ray, out Point3D[] result)
{
double cx = this.center.X;
double cy = this.center.Y;
double cz = this.center.Z;
double r = this.radius;
double x1 = ray.Origin.X;
double y1 = ray.Origin.Y;
double z1 = ray.Origin.Z;
double dx = ray.Direction.X;
double dy = ray.Direction.Y;
double dz = ray.Direction.Z;
// Quadratic solving
double a = (dx * dx) + (dy * dy) + (dz * dz);
double b = (2 * dx * (x1 - cx)) + (2 * dy * (y1 - cy)) + (2 * dz * (z1 - cz));
double c = (x1 * x1) + (y1 * y1) + (z1 * z1) + (cx * cx) + (cz * cz) + (cy * cy) - (2 * ((cy * y1) + (cz * z1) + (cx * x1))) - (r * r);
// Discriminant
double q = (b * b) - (4 * a * c);
// We have at least one possible intersection
if (q >= 0)
{
double q2 = Math.Sqrt((b * b) - (4 * a * c));
// First root
double t1 = (-b + q2) / (2 * a);
// Second root
double t2 = (-b - q2) / (2 * a);
if (t1 >= 0 && t2 >= 0 && !t1.Equals(t2))
{
var i1 = new Point3D(x1 + (dx * t1), y1 + (dy * t1), z1 + (dz * t1));
var i2 = new Point3D(x1 + (dx * t2), y1 + (dy * t2), z1 + (dz * t2));
result = t1 < t2 ? new[] { i1, i2 } : new[] { i2, i1 };
return true;
}
if (t1 >= 0)
{
var i1 = new Point3D(x1 + (dx * t1), y1 + (dy * t1), z1 + (dz * t1));
result = new[] { i1 };
return true;
}
if (t2 >= 0)
{
var i2 = new Point3D(x1 + (dx * t2), y1 + (dy * t2), z1 + (dz * t2));
result = new[] { i2 };
return true;
}
}
result = null;
return false;
}
public void XmlTests(string ps, string vs, bool asElements, string xml)
{
var ray = new Ray3D(Point3D.Parse(ps), UnitVector3D.Parse(vs));
AssertXml.XmlRoundTrips(ray, xml, (e, a) => AssertGeometry.AreEqual(e, a, 1e-6));
}
