double R3SignedDistance(const R3Plane& plane, const R3Box& box)
{
// Return signed distance from plane to box
int ix = (plane.Normal().X() > 0) ? 0 : 1;
int iy = (plane.Normal().Y() > 0) ? 0 : 1;
int iz = (plane.Normal().Z() > 0) ? 0 : 1;
double d1 = R3SignedDistance(plane, box.Corner(ix, iy, iz));
if (d1 >= 0) return d1;
double d2 = R3SignedDistance(plane, box.Corner(1-ix, 1-iy, 1-iz));
if (d2 < 0) return d2;
else return 0.0;
}
RNClassID R3Span::
Clip(const R3Plane& plane)
{
// Characterize endpoints with respect to plane
RNScalar d1 = R3SignedDistance(plane, Start());
RNScalar d2 = R3SignedDistance(plane, End());
// Clip span to plane
if (RNIsNegative(d1)) {
if (RNIsNegative(d2)) {
// Both points are below plane ???
Reset(Start(), Start());
return RN_NULL_CLASS_ID;
}
else if (RNIsPositive(d2)) {
// Start is below, end is above -- move start to plane
Reset((Start() * d2 + End() * -d1) / (d2 - d1), End());
return R3_SPAN_CLASS_ID;
}
else {
// Start is below, end is on -- move start to end
Reset(End(), End());
return R3_POINT_CLASS_ID;
}
}
else if (RNIsPositive(d1)) {
if (RNIsNegative(d2)) {
// Start is above, end is below -- move end to plane
Reset(Start(), (Start() * -d2 + End() * d1) / (d1 - d2));
return R3_SPAN_CLASS_ID;
}
else {
// Start is above, end is on or above
return R3_SPAN_CLASS_ID;
}
}
else {
if (RNIsNegative(d2)) {
// Start is on, end is below -- move end to start
Reset(Start(), Start());
return R3_POINT_CLASS_ID;
}
else {
// Start is on, end is on or above
return R3_SPAN_CLASS_ID;
}
}
}
void R3Point::
Project(const R3Plane& plane)
{
// Move point to closest point on plane
RNScalar d = R3SignedDistance(plane, *this);
*this += plane.Normal() * -d;
}
void R3Point::
Mirror(const R3Plane& plane)
{
// Mirror point across plane
RNScalar d = R3SignedDistance(plane, *this);
*this += plane.Normal() * (-2.0 * d);
}
double R3Distance(const R3Plane& plane, const R3Box& box)
{
// Return distance from plane to box
double d = R3SignedDistance(plane, box);
if (d > 0) return d;
else if (d < 0) return -d;
else return 0.0;
}
double R3Distance(const R3Plane& plane1, const R3Plane& plane2)
{
// Return distance from plane to plane
double d = R3SignedDistance(plane1, plane2);
if (d > 0) return d;
else if (d < 0) return -d;
else return 0.0;
}
double R3Distance(const R3Line& line, const R3Plane& plane)
{
// Return distance from line to plane
double d = R3SignedDistance(plane, line);
if (d > 0) return d;
else if (d < 0) return -d;
else return 0.0;
}
double R3Distance(const R3Ray& ray, const R3Plane& plane)
{
// Return distance from ray to plane
double d = R3SignedDistance(plane, ray);
if (d > 0) return d;
else if (d < 0) return -d;
else return 0.0;
}
double R3Distance(const R3Point& point, const R3Plane& plane)
{
// Return distance from point to plane
double d = R3SignedDistance(plane, point);
if (d > 0) return d;
else if (d < 0) return -d;
else return 0.0;
}
double R3SignedDistance(const R3Plane& plane, const R3Line& line)
{
// Return signed distance from plane to line
if (plane.Normal().Dot(line.Vector()) == 0) {
// Plane and line are parallel
return R3SignedDistance(plane, line.Point());
}
else {
// Plane and line are not parallel
return 0.0;
}
}
RNBoolean R3Contains(const R3Halfspace& halfspace, const R3Circle& circle)
{
// Return whether halfspace contains circle
RNScalar d = R3SignedDistance(halfspace.Plane(), circle.Center());
if (RNIsNegative(d)) return FALSE;
else if (RNIsGreater(d, circle.Radius())) return TRUE;
else {
RNScalar cos_theta = halfspace.Plane().Normal().Dot(circle.Normal());
if (cos_theta < 0.0) cos_theta = halfspace.Plane().Normal().Dot(-circle.Normal());
return (RNIsGreaterOrEqual(d, cos_theta * circle.Radius()));
}
}
double R3SignedDistance(const R3Plane& plane, const R3Segment& segment)
{
// Return signed distance from plane to segment
double d1 = R3SignedDistance(plane, segment.Start());
if (d1 > 0) {
// Start point is above plane
double d2 = R3SignedDistance(plane, segment.End());
if (d2 > 0) return ((d1 > d2) ? d2 : d1);
else return 0.0;
}
else if (d1 < 0) {
// Start point is below plane
double d2 = R3SignedDistance(plane, segment.End());
if (d2 < 0) return ((d1 > d2) ? d1 : d2);
else return 0.0;
}
else {
// Start point is on plane
return 0.0;
}
}
RNBoolean R3Contains(const R3Triangle& triangle, const R3Point& point)
{
// Check whether triangle bounding shape contains point
if (!R3Contains(triangle.Box(), point)) return FALSE;
// Check whether triangle plane contains point
if (!R3Contains(triangle.Plane(), point)) return FALSE;
// Compute whether point is on correct side of each edge
const R3Point& p0 = triangle.Vertex(0)->Position();
const R3Point& p1 = triangle.Vertex(1)->Position();
const R3Point& p2 = triangle.Vertex(2)->Position();
R3Plane h01(p1, triangle.Normal(), p1 - p0);
if (RNIsNegative(R3SignedDistance(h01, point))) return FALSE;
R3Plane plane01(p1, triangle.Normal(), p1 - p0);
if (RNIsNegative(R3SignedDistance(plane01, point))) return FALSE;
R3Plane plane12(p2, triangle.Normal(), p2 - p1);
if (RNIsNegative(R3SignedDistance(plane12, point))) return FALSE;
R3Plane plane20(p0, triangle.Normal(), p0 - p2);
if (RNIsNegative(R3SignedDistance(plane20, point))) return FALSE;
// Triangle contains point
return TRUE;
}
double R3SignedDistance(const R3Plane& plane, const R3Ray& ray)
{
// Return signed distance from plane to ray
double d1 = R3SignedDistance(plane, ray.Start());
if (d1 > 0) {
// Start point is above plane
double dot = ray.Vector().Dot(plane.Normal());
if (dot < 0) return 0.0;
else return d1;
}
else if (d1 < 0) {
// Start point is below plane
double dot = ray.Vector().Dot(plane.Normal());
if (dot > 0) return 0.0;
else return d1;
}
else {
// Start point is on plane
return 0.0;
}
}
void R3MeshSearchTree::
FindAll(const R3Point& query_position, const R3Vector& query_normal, RNArray<R3MeshIntersection *>& hits,
RNScalar min_distance_squared, RNScalar max_distance_squared,
int (*IsCompatible)(const R3Point&, const R3Vector&, R3Mesh *, R3MeshFace *, void *), void *compatible_data,
R3MeshFace *face) const
{
// Check distance to plane
const R3Plane& plane = mesh->FacePlane(face);
RNScalar plane_signed_distance = R3SignedDistance(plane, query_position);
RNScalar plane_distance_squared = plane_signed_distance * plane_signed_distance;
if (plane_distance_squared >= max_distance_squared) return;
// Check distance to bounding box
RNScalar bbox_distance_squared = DistanceSquared(query_position, mesh->FaceBBox(face), max_distance_squared);
if (bbox_distance_squared >= max_distance_squared) return;
// Check compatibility
if (IsCompatible) {
if (!(*IsCompatible)(query_position, query_normal, mesh, face, compatible_data)) return;
}
// Get face vertices
R3MeshVertex *v0 = mesh->VertexOnFace(face, 0);
R3MeshVertex *v1 = mesh->VertexOnFace(face, 1);
R3MeshVertex *v2 = mesh->VertexOnFace(face, 2);
// Get vertex positions
const R3Point& p0 = mesh->VertexPosition(v0);
const R3Point& p1 = mesh->VertexPosition(v1);
const R3Point& p2 = mesh->VertexPosition(v2);
// Project query point onto face plane
const R3Vector& face_normal = mesh->FaceNormal(face);
R3Point plane_point = query_position - plane_signed_distance * face_normal;
// Check sides of edges
R3Vector e0 = p1 - p0;
e0.Normalize();
R3Vector n0 = mesh->FaceNormal(face) % e0;
R3Plane s0(p0, n0);
RNScalar b0 = R3SignedDistance(s0, plane_point);
R3Vector e1 = p2 - p1;
e1.Normalize();
R3Vector n1 = mesh->FaceNormal(face) % e1;
R3Plane s1(p1, n1);
RNScalar b1 = R3SignedDistance(s1, plane_point);
R3Vector e2 = p0 - p2;
e2.Normalize();
R3Vector n2 = mesh->FaceNormal(face) % e2;
R3Plane s2(p2, n2);
RNScalar b2 = R3SignedDistance(s2, plane_point);
// Initialize hit info
R3MeshIntersection hit;
hit.type = R3_MESH_NULL_TYPE;
// Consider plane_point's position in relation to edges of the triangle
if ((b0 >= 0) && (b1 >= 0) && (b2 >= 0)) {
// Point is inside face
if (plane_distance_squared >= min_distance_squared) {
hit.type = R3_MESH_FACE_TYPE;
hit.vertex = NULL;
hit.edge = NULL;
hit.face = face;
hit.point = plane_point;
hit.t = sqrt(plane_distance_squared);
}
}
else {
// Point is outside face -- check each edge
if (b0 < 0) {
// Outside edge0
R3Vector edge_vector = p1 - p0;
RNScalar edge_length = edge_vector.Length();
if (edge_length > 0) {
edge_vector /= edge_length;
R3Vector point_vector = plane_point - p0;
RNScalar t = edge_vector.Dot(point_vector);
if (t <= 0) {
RNScalar distance_squared = DistanceSquared(query_position, p0);
if ((distance_squared >= min_distance_squared) && (distance_squared < max_distance_squared)) {
hit.type = R3_MESH_VERTEX_TYPE;
hit.vertex = v0;
hit.edge = mesh->EdgeOnVertex(hit.vertex, face);
hit.face = face;
hit.point = p0;
hit.t = sqrt(distance_squared);
max_distance_squared = distance_squared;
}
}
else if (t >= edge_length) {
RNScalar distance_squared = DistanceSquared(query_position, p1);
if ((distance_squared >= min_distance_squared) && (distance_squared < max_distance_squared)) {
hit.type = R3_MESH_VERTEX_TYPE;
hit.vertex = v1;
hit.edge = mesh->EdgeOnVertex(hit.vertex, face);
hit.face = face;
hit.point = p1;
hit.t = sqrt(distance_squared);
max_distance_squared = distance_squared;
}
}
else {
R3Point point = p0 + t * edge_vector;
RNScalar distance_squared = DistanceSquared(query_position, point);
if ((distance_squared >= min_distance_squared) && (distance_squared < max_distance_squared)) {
hit.type = R3_MESH_EDGE_TYPE;
hit.vertex = NULL;
hit.edge = mesh->EdgeOnFace(face, 0);
hit.face = face;
hit.point = point;
hit.t = sqrt(distance_squared);
max_distance_squared = distance_squared;
}
}
}
}
if (b1 < 0) {
// Outside edge1
R3Vector edge_vector = p2 - p1;
RNScalar edge_length = edge_vector.Length();
if (edge_length > 0) {
edge_vector /= edge_length;
R3Vector point_vector = plane_point - p1;
RNScalar t = edge_vector.Dot(point_vector);
if (t <= 0) {
RNScalar distance_squared = DistanceSquared(query_position, p1);
if ((distance_squared >= min_distance_squared) && (distance_squared < max_distance_squared)) {
hit.type = R3_MESH_VERTEX_TYPE;
hit.vertex = v1;
hit.edge = mesh->EdgeOnVertex(hit.vertex, face);
hit.face = face;
hit.point = p1;
hit.t = sqrt(distance_squared);
max_distance_squared = distance_squared;
}
}
else if (t >= edge_length) {
RNScalar distance_squared = DistanceSquared(query_position, p2);
if ((distance_squared >= min_distance_squared) && (distance_squared < max_distance_squared)) {
hit.type = R3_MESH_VERTEX_TYPE;
hit.vertex = v2;
hit.edge = mesh->EdgeOnVertex(hit.vertex, face);
hit.face = face;
hit.point = p2;
hit.t = sqrt(distance_squared);
max_distance_squared = distance_squared;
}
}
else {
R3Point point = p1 + t * edge_vector;
RNScalar distance_squared = DistanceSquared(query_position, point);
if ((distance_squared >= min_distance_squared) && (distance_squared < max_distance_squared)) {
hit.type = R3_MESH_EDGE_TYPE;
hit.vertex = NULL;
hit.edge = mesh->EdgeOnFace(face, 1);
hit.face = face;
hit.point = point;
hit.t = sqrt(distance_squared);
max_distance_squared = distance_squared;
}
}
}
}
if (b2 < 0) {
// Outside edge2
R3Vector edge_vector = p0 - p2;
RNScalar edge_length = edge_vector.Length();
if (edge_length > 0) {
edge_vector /= edge_length;
R3Vector point_vector = plane_point - p2;
RNScalar t = edge_vector.Dot(point_vector);
if (t <= 0) {
RNScalar distance_squared = DistanceSquared(query_position, p2);
if ((distance_squared >= min_distance_squared) && (distance_squared < max_distance_squared)) {
hit.type = R3_MESH_VERTEX_TYPE;
hit.vertex = v2;
hit.edge = mesh->EdgeOnVertex(hit.vertex, face);
hit.face = face;
hit.point = p2;
hit.t = sqrt(distance_squared);
max_distance_squared = distance_squared;
}
}
else if (t >= edge_length) {
RNScalar distance_squared = DistanceSquared(query_position, p0);
if ((distance_squared >= min_distance_squared) && (distance_squared < max_distance_squared)) {
hit.type = R3_MESH_VERTEX_TYPE;
hit.vertex = v0;
hit.edge = mesh->EdgeOnVertex(hit.vertex, face);
hit.face = face;
hit.point = p0;
hit.t = sqrt(distance_squared);
max_distance_squared = distance_squared;
}
}
else {
R3Point point = p2 + t * edge_vector;
RNScalar distance_squared = DistanceSquared(query_position, point);
if ((distance_squared >= min_distance_squared) && (distance_squared < max_distance_squared)) {
hit.type = R3_MESH_EDGE_TYPE;
hit.vertex = NULL;
hit.edge = mesh->EdgeOnFace(face, 2);
hit.face = face;
hit.point = point;
hit.t = sqrt(distance_squared);
max_distance_squared = distance_squared;
}
}
}
}
}
// Insert hit
if (hit.type != R3_MESH_NULL_TYPE) {
hits.Insert(new R3MeshIntersection(hit));
}
}
RNBoolean R3Contains(const R3Halfspace& halfspace, const R3Sphere& sphere)
{
// Return whether halfspace contains sphere
RNLength d = R3SignedDistance(halfspace.Plane(), sphere.Center());
return RNIsGreaterOrEqual(d, sphere.Radius());
}
RNBoolean R3Contains(const R3Halfspace& halfspace, const R3Point& point)
{
// Return whether halfspace contains point
return (RNIsPositiveOrZero(R3SignedDistance(halfspace.Plane(), point)));
}
RNBoolean R3Contains(const R3Plane& plane, const R3Point& point)
{
// Return whether plane contains point
return RNIsZero(R3SignedDistance(plane, point));
}