Gabriele Cosmo
1258 lines
32 KiB
C++
1258 lines
32 KiB
C++
//
|
|
// ********************************************************************
|
|
// * This Software is part of the AIDA Unified Solids Library package *
|
|
// * See: https://aidasoft.web.cern.ch/USolids *
|
|
// ********************************************************************
|
|
//
|
|
// $Id:$
|
|
//
|
|
// --------------------------------------------------------------------
|
|
//
|
|
// UPolyhedraSide
|
|
//
|
|
// 19.09.13 Marek Gayer
|
|
// Created from original implementation in Geant4
|
|
// --------------------------------------------------------------------
|
|
|
|
#include "UUtils.hh"
|
|
#include <string>
|
|
#include <cmath>
|
|
#include <sstream>
|
|
#include "UPolyhedraSide.hh"
|
|
#include "UIntersectingCone.hh"
|
|
|
|
|
|
//
|
|
// Constructor
|
|
//
|
|
// Values for r1,z1 and r2,z2 should be specified in clockwise
|
|
// order in (r,z).
|
|
//
|
|
UPolyhedraSide::UPolyhedraSide(const UPolyhedraSideRZ* prevRZ,
|
|
const UPolyhedraSideRZ* tail,
|
|
const UPolyhedraSideRZ* head,
|
|
const UPolyhedraSideRZ* nextRZ,
|
|
int theNumSide,
|
|
double thePhiStart,
|
|
double thePhiTotal,
|
|
bool thePhiIsOpen,
|
|
bool isAllBehind)
|
|
{
|
|
kCarTolerance = VUSolid::Tolerance();
|
|
fSurfaceArea = 0.;
|
|
fPhi.first.Set(0);
|
|
fPhi.second = 0.0;
|
|
|
|
//
|
|
// Record values
|
|
//
|
|
r[0] = tail->r;
|
|
z[0] = tail->z;
|
|
r[1] = head->r;
|
|
z[1] = head->z;
|
|
|
|
double phiTotal;
|
|
|
|
//
|
|
// Set phi to our convention
|
|
//
|
|
startPhi = thePhiStart;
|
|
while (startPhi < 0.0) startPhi += 2 * UUtils::kPi;
|
|
|
|
phiIsOpen = thePhiIsOpen;
|
|
phiTotal = (phiIsOpen) ? thePhiTotal : 2 * UUtils::kPi;
|
|
|
|
allBehind = isAllBehind;
|
|
|
|
//
|
|
// Make our intersecting cone
|
|
//
|
|
cone = new UIntersectingCone(r, z);
|
|
|
|
//
|
|
// Construct side plane vector Set
|
|
//
|
|
numSide = theNumSide;
|
|
deltaPhi = phiTotal / theNumSide;
|
|
endPhi = startPhi + phiTotal;
|
|
|
|
vecs = new UPolyhedraSideVec[numSide];
|
|
|
|
edges = new UPolyhedraSideEdge[phiIsOpen ? numSide + 1 : numSide];
|
|
|
|
//
|
|
// ...this is where we start
|
|
//
|
|
double phi = startPhi;
|
|
UVector3 a1(r[0]*std::cos(phi), r[0]*std::sin(phi), z[0]),
|
|
b1(r[1]*std::cos(phi), r[1]*std::sin(phi), z[1]),
|
|
c1(prevRZ->r * std::cos(phi), prevRZ->r * std::sin(phi), prevRZ->z),
|
|
d1(nextRZ->r * std::cos(phi), nextRZ->r * std::sin(phi), nextRZ->z),
|
|
a2, b2, c2, d2;
|
|
UPolyhedraSideEdge* edge = edges;
|
|
|
|
UPolyhedraSideVec* vec = vecs;
|
|
do
|
|
{
|
|
//
|
|
// ...this is where we are going
|
|
//
|
|
phi += deltaPhi;
|
|
a2 = UVector3(r[0] * std::cos(phi), r[0] * std::sin(phi), z[0]);
|
|
b2 = UVector3(r[1] * std::cos(phi), r[1] * std::sin(phi), z[1]);
|
|
c2 = UVector3(prevRZ->r * std::cos(phi), prevRZ->r * std::sin(phi), prevRZ->z);
|
|
d2 = UVector3(nextRZ->r * std::cos(phi), nextRZ->r * std::sin(phi), nextRZ->z);
|
|
|
|
UVector3 tt;
|
|
|
|
//
|
|
// ...build some relevant vectors.
|
|
// the point is to sacrifice a little memory with precalcs
|
|
// to gain speed
|
|
//
|
|
vec->center = 0.25 * (a1 + a2 + b1 + b2);
|
|
|
|
tt = b2 + b1 - a2 - a1;
|
|
vec->surfRZ = tt.Unit();
|
|
if (vec == vecs) lenRZ = 0.25 * tt.Mag();
|
|
|
|
tt = b2 - b1 + a2 - a1;
|
|
vec->surfPhi = tt.Unit();
|
|
if (vec == vecs)
|
|
{
|
|
lenPhi[0] = 0.25 * tt.Mag();
|
|
tt = b2 - b1;
|
|
lenPhi[1] = (0.5 * tt.Mag() - lenPhi[0]) / lenRZ;
|
|
}
|
|
|
|
tt = vec->surfPhi.Cross(vec->surfRZ);
|
|
vec->normal = tt.Unit();
|
|
|
|
//
|
|
// ...edge normals are the average of the normals of
|
|
// the two faces they connect.
|
|
//
|
|
// ...edge normals are necessary if we are to accurately
|
|
// decide if a point is "inside" a face. For non-convex
|
|
// shapes, it is absolutely necessary to know information
|
|
// on adjacent faces to accurate determine this.
|
|
//
|
|
// ...we don't need them for the phi edges, since that
|
|
// information is taken care of internally. The r/z edges,
|
|
// however, depend on the adjacent UPolyhedraSide.
|
|
//
|
|
UVector3 a12, adj;
|
|
|
|
a12 = a2 - a1;
|
|
|
|
adj = 0.5 * (c1 + c2 - a1 - a2);
|
|
adj = adj.Cross(a12);
|
|
adj = adj.Unit() + vec->normal;
|
|
vec->edgeNorm[0] = adj.Unit();
|
|
|
|
a12 = b1 - b2;
|
|
adj = 0.5 * (d1 + d2 - b1 - b2);
|
|
adj = adj.Cross(a12);
|
|
adj = adj.Unit() + vec->normal;
|
|
vec->edgeNorm[1] = adj.Unit();
|
|
|
|
//
|
|
// ...the corners are crucial. It is important that
|
|
// they are calculated consistently for adjacent
|
|
// UPolyhedraSides, to avoid gaps caused by roundoff.
|
|
//
|
|
vec->edges[0] = edge;
|
|
edge->corner[0] = a1;
|
|
edge->corner[1] = b1;
|
|
edge++;
|
|
vec->edges[1] = edge;
|
|
|
|
a1 = a2;
|
|
b1 = b2;
|
|
c1 = c2;
|
|
d1 = d2;
|
|
}
|
|
while (++vec < vecs + numSide);
|
|
|
|
//
|
|
// Clean up hanging edge
|
|
//
|
|
if (phiIsOpen)
|
|
{
|
|
edge->corner[0] = a2;
|
|
edge->corner[1] = b2;
|
|
}
|
|
else
|
|
{
|
|
vecs[numSide - 1].edges[1] = edges;
|
|
}
|
|
|
|
//
|
|
// Go back and fill in remaining fields in edges
|
|
//
|
|
vec = vecs;
|
|
UPolyhedraSideVec* prev = vecs + numSide - 1;
|
|
do
|
|
{
|
|
edge = vec->edges[0]; // The edge between prev and vec
|
|
|
|
//
|
|
// Okay: edge normal is average of normals of adjacent faces
|
|
//
|
|
UVector3 eNorm = vec->normal + prev->normal;
|
|
edge->normal = eNorm.Unit();
|
|
|
|
//
|
|
// Vertex normal is average of norms of adjacent surfaces (all four)
|
|
// However, vec->edgeNorm is Unit vector in some direction
|
|
// as the sum of normals of adjacent PolyhedraSide with vec.
|
|
// The normalization used for this vector should be the same
|
|
// for vec and prev.
|
|
//
|
|
eNorm = vec->edgeNorm[0] + prev->edgeNorm[0];
|
|
edge->cornNorm[0] = eNorm.Unit();
|
|
|
|
eNorm = vec->edgeNorm[1] + prev->edgeNorm[1];
|
|
edge->cornNorm[1] = eNorm.Unit();
|
|
}
|
|
while (prev = vec, ++vec < vecs + numSide);
|
|
|
|
if (phiIsOpen)
|
|
{
|
|
// double rFact = std::cos(0.5*deltaPhi);
|
|
//
|
|
// If phi is open, we need to patch up normals of the
|
|
// first and last edges and their corresponding
|
|
// vertices.
|
|
//
|
|
// We use vectors that are in the plane of the
|
|
// face. This should be safe.
|
|
//
|
|
vec = vecs;
|
|
|
|
UVector3 normvec = vec->edges[0]->corner[0]
|
|
- vec->edges[0]->corner[1];
|
|
normvec = normvec.Cross(vec->normal);
|
|
if (normvec.Dot(vec->surfPhi) > 0) normvec = -normvec;
|
|
|
|
vec->edges[0]->normal = normvec.Unit();
|
|
|
|
vec->edges[0]->cornNorm[0] = (vec->edges[0]->corner[0]
|
|
- vec->center).Unit();
|
|
vec->edges[0]->cornNorm[1] = (vec->edges[0]->corner[1]
|
|
- vec->center).Unit();
|
|
|
|
//
|
|
// Repeat for ending phi
|
|
//
|
|
vec = vecs + numSide - 1;
|
|
|
|
normvec = vec->edges[1]->corner[0] - vec->edges[1]->corner[1];
|
|
normvec = normvec.Cross(vec->normal);
|
|
if (normvec.Dot(vec->surfPhi) < 0) normvec = -normvec;
|
|
|
|
vec->edges[1]->normal = normvec.Unit();
|
|
|
|
vec->edges[1]->cornNorm[0] = (vec->edges[1]->corner[0]
|
|
- vec->center).Unit();
|
|
vec->edges[1]->cornNorm[1] = (vec->edges[1]->corner[1]
|
|
- vec->center).Unit();
|
|
}
|
|
|
|
//
|
|
// edgeNorm is the factor one multiplies the distance along vector phi
|
|
// on the surface of one of our sides in order to calculate the distance
|
|
// from the edge. (see routine DistanceAway)
|
|
//
|
|
edgeNorm = 1.0 / std::sqrt(1.0 + lenPhi[1] * lenPhi[1]);
|
|
}
|
|
|
|
|
|
//
|
|
// Fake default constructor - sets only member data and allocates memory
|
|
// for usage restricted to object persistency.
|
|
//
|
|
UPolyhedraSide::UPolyhedraSide(__void__&)
|
|
: numSide(0), startPhi(0.), deltaPhi(0.), endPhi(0.),
|
|
phiIsOpen(false), allBehind(false), cone(0), vecs(0), edges(0),
|
|
lenRZ(0.), edgeNorm(0.), kCarTolerance(0.), fSurfaceArea(0.)
|
|
{
|
|
r[0] = r[1] = 0.;
|
|
z[0] = z[1] = 0.;
|
|
lenPhi[0] = lenPhi[1] = 0.;
|
|
}
|
|
|
|
|
|
//
|
|
// Destructor
|
|
//
|
|
UPolyhedraSide::~UPolyhedraSide()
|
|
{
|
|
delete cone;
|
|
delete [] vecs;
|
|
delete [] edges;
|
|
}
|
|
|
|
|
|
//
|
|
// Copy constructor
|
|
//
|
|
UPolyhedraSide::UPolyhedraSide(const UPolyhedraSide& source)
|
|
: UVCSGface()
|
|
{
|
|
CopyStuff(source);
|
|
}
|
|
|
|
|
|
//
|
|
// Assignment operator
|
|
//
|
|
UPolyhedraSide& UPolyhedraSide::operator=(const UPolyhedraSide& source)
|
|
{
|
|
if (this == &source) return *this;
|
|
|
|
delete cone;
|
|
delete [] vecs;
|
|
delete [] edges;
|
|
|
|
CopyStuff(source);
|
|
|
|
return *this;
|
|
}
|
|
|
|
|
|
//
|
|
// CopyStuff
|
|
//
|
|
void UPolyhedraSide::CopyStuff(const UPolyhedraSide& source)
|
|
{
|
|
//
|
|
// The simple stuff
|
|
//
|
|
numSide = source.numSide;
|
|
r[0] = source.r[0];
|
|
r[1] = source.r[1];
|
|
z[0] = source.z[0];
|
|
z[1] = source.z[1];
|
|
startPhi = source.startPhi;
|
|
deltaPhi = source.deltaPhi;
|
|
endPhi = source.endPhi;
|
|
phiIsOpen = source.phiIsOpen;
|
|
allBehind = source.allBehind;
|
|
|
|
lenRZ = source.lenRZ;
|
|
lenPhi[0] = source.lenPhi[0];
|
|
lenPhi[1] = source.lenPhi[1];
|
|
edgeNorm = source.edgeNorm;
|
|
|
|
kCarTolerance = source.kCarTolerance;
|
|
fSurfaceArea = source.fSurfaceArea;
|
|
|
|
cone = new UIntersectingCone(*source.cone);
|
|
|
|
//
|
|
// Duplicate edges
|
|
//
|
|
int numEdges = phiIsOpen ? numSide + 1 : numSide;
|
|
edges = new UPolyhedraSideEdge[numEdges];
|
|
|
|
UPolyhedraSideEdge* edge = edges,
|
|
*sourceEdge = source.edges;
|
|
do
|
|
{
|
|
*edge = *sourceEdge;
|
|
}
|
|
while (++sourceEdge, ++edge < edges + numEdges);
|
|
|
|
//
|
|
// Duplicate vecs
|
|
//
|
|
vecs = new UPolyhedraSideVec[numSide];
|
|
|
|
UPolyhedraSideVec* vec = vecs,
|
|
*sourceVec = source.vecs;
|
|
do
|
|
{
|
|
*vec = *sourceVec;
|
|
vec->edges[0] = edges + (sourceVec->edges[0] - source.edges);
|
|
vec->edges[1] = edges + (sourceVec->edges[1] - source.edges);
|
|
}
|
|
while (++sourceVec, ++vec < vecs + numSide);
|
|
}
|
|
|
|
|
|
//
|
|
// Intersect
|
|
//
|
|
// Decide if a line intersects the face.
|
|
//
|
|
// Arguments:
|
|
// p = (in) starting point of line segment
|
|
// v = (in) direction of line segment (assumed a Unit vector)
|
|
// A, B = (in) 2d transform variables (see note top of file)
|
|
// normSign = (in) desired sign for Dot product with normal (see below)
|
|
// surfTolerance = (in) minimum distance from the surface
|
|
// vecs = (in) Vector Set array
|
|
// distance = (out) distance to surface furfilling all requirements
|
|
// distFromSurface = (out) distance from the surface
|
|
// thisNormal = (out) normal vector of the intersecting surface
|
|
//
|
|
// Return value:
|
|
// true if an intersection is found. Otherwise, output parameters are
|
|
// undefined.
|
|
//
|
|
// Notes:
|
|
// * normSign: if we are "inside" the shape and only want to find out how far
|
|
// to leave the shape, we only want to consider intersections with surfaces in
|
|
// which the trajectory is leaving the shape. Since the normal vectors to the
|
|
// surface always point outwards from the inside, this means we want the Dot
|
|
// product of the trajectory direction v and the normal of the side normals[i]
|
|
// to be positive. Thus, we should specify normSign as +1.0. Otherwise, if
|
|
// we are outside and want to go in, normSign should be Set to -1.0.
|
|
// Don't Set normSign to zero, or you will get no intersections!
|
|
//
|
|
// * surfTolerance: see notes on argument "surfTolerance" in routine
|
|
// "IntersectSidePlane".
|
|
// ----HOWEVER---- We should *not* apply this surface tolerance if the
|
|
// starting point is not within phi or z of the surface. Specifically,
|
|
// if the starting point p angle in x/y places it on a separate side from the
|
|
// intersection or if the starting point p is outside the z bounds of the
|
|
// segment, surfTolerance must be ignored or we should *always* accept the
|
|
// intersection!
|
|
// This is simply because the sides do not have infinite extent.
|
|
//
|
|
//
|
|
bool UPolyhedraSide::Distance(const UVector3& p,
|
|
const UVector3& v,
|
|
bool outgoing,
|
|
double surfTolerance,
|
|
double& distance,
|
|
double& distFromSurface,
|
|
UVector3& normal,
|
|
bool& isAllBehind)
|
|
{
|
|
int segment = -1;
|
|
|
|
//
|
|
// Testing the intersection of individual phi faces is
|
|
// pretty straight forward. The simple thing therefore is to
|
|
// form a loop and check them all in sequence.
|
|
//
|
|
// We try to quickly decide
|
|
// which face would be intersected. One can make a very
|
|
// good guess by using the intersection with a cone.
|
|
// However, this is only reliable in 99% of the cases.
|
|
//
|
|
// We make a decent guess as to the one or
|
|
// two potential faces might get intersected, and then
|
|
// test them. If we have the wrong face, use the test
|
|
// to make a better guess.
|
|
//
|
|
// Since we might have two guesses, form a queue of
|
|
// potential intersecting faces. Keep an array of
|
|
// already tested faces to avoid doing one more than
|
|
// once.
|
|
//
|
|
// Result: at worst, an iterative search. On average,
|
|
// a little more than two tests would be required.
|
|
//
|
|
|
|
if (numSide > 5)
|
|
{
|
|
//todo: maybe we could even use the second solution? how much also the second would be relevant???
|
|
double s1, s2;
|
|
int solutions = cone->LineHitsCone(p, v, s1, s2);
|
|
if (!solutions) return false;
|
|
if (solutions == 2 && s2 > 0 && (s2 < s1 || s1 < 0))
|
|
s1 = s2;
|
|
|
|
segment = PhiSegment(std::atan2(p.y + s1 * v.y, p.x + s1 * v.x));
|
|
}
|
|
|
|
UVector3 q = p + v;
|
|
UVector3 ps, delta, qa, qb, qc, qd;
|
|
|
|
int face = -1;
|
|
double normSign = outgoing ? 1 : -1;
|
|
|
|
do
|
|
{
|
|
if (face == segment) continue;
|
|
UPolyhedraSideVec& vec = (face == -1) ? vecs[segment] : vecs[face];
|
|
//
|
|
// Correct normal?
|
|
//
|
|
double dotProd = normSign * v.Dot(vec.normal);
|
|
if (dotProd <= 0) continue;
|
|
|
|
//
|
|
// Is this face in front of the point along the trajectory?
|
|
//
|
|
delta = p - vec.center;
|
|
distFromSurface = -normSign * delta.Dot(vec.normal);
|
|
|
|
if (distFromSurface < -surfTolerance) continue;
|
|
|
|
//
|
|
// phi
|
|
// c -------- d ^
|
|
// | | |
|
|
// a -------- b +---> r/z
|
|
//
|
|
//
|
|
// Do we remain on this particular segment?
|
|
//
|
|
qc = q - vec.edges[1]->corner[0];
|
|
qd = q - vec.edges[1]->corner[1];
|
|
|
|
if (normSign * qc.Cross(qd).Dot(v) < 0) continue;
|
|
|
|
qa = q - vec.edges[0]->corner[0];
|
|
qb = q - vec.edges[0]->corner[1];
|
|
|
|
if (normSign * qa.Cross(qb).Dot(v) > 0) continue;
|
|
|
|
//
|
|
// We found the one and only segment we might be intersecting.
|
|
// Do we remain within r/z bounds?
|
|
//
|
|
|
|
if (r[0] > 1 / UUtils::kInfinity && normSign * qa.Cross(qc).Dot(v) < 0) return false;
|
|
if (r[1] > 1 / UUtils::kInfinity && normSign * qb.Cross(qd).Dot(v) > 0) return false;
|
|
|
|
//
|
|
// We allow the face to be slightly behind the trajectory
|
|
// (surface tolerance) only if the point p is within
|
|
// the vicinity of the face
|
|
//
|
|
if (distFromSurface < 0)
|
|
{
|
|
ps = p - vec.center;
|
|
|
|
double rz = ps.Dot(vec.surfRZ);
|
|
if (std::fabs(rz) > lenRZ + surfTolerance) return false;
|
|
|
|
double pp = ps.Dot(vec.surfPhi);
|
|
if (std::fabs(pp) > lenPhi[0] + lenPhi[1]*rz + surfTolerance) return false;
|
|
}
|
|
|
|
//
|
|
// Intersection found. Return answer.
|
|
//
|
|
distance = distFromSurface / dotProd;
|
|
normal = vec.normal;
|
|
isAllBehind = allBehind;
|
|
return true;
|
|
}
|
|
while (++face < numSide);
|
|
|
|
//
|
|
// Oh well. Better luck next time.
|
|
//
|
|
return false;
|
|
}
|
|
|
|
|
|
double UPolyhedraSide::Safety(const UVector3& p, bool outgoing)
|
|
{
|
|
double normSign = outgoing ? -1 : +1;
|
|
|
|
//
|
|
// Try the closest phi segment first
|
|
//
|
|
int iPhi = ClosestPhiSegment(GetPhi(p));
|
|
|
|
UVector3 pdotc = p - vecs[iPhi].center;
|
|
double normDist = pdotc.Dot(vecs[iPhi].normal);
|
|
|
|
if (normSign * normDist > -0.5 * VUSolid::Tolerance())
|
|
{
|
|
return DistanceAway(p, vecs[iPhi], &normDist);
|
|
}
|
|
|
|
//
|
|
// Now we have an interesting problem... do we try to find the
|
|
// closest facing side??
|
|
//
|
|
// Considered carefully, the answer is no. We know that if we
|
|
// are asking for the distance out, we are supposed to be inside,
|
|
// and vice versa.
|
|
//
|
|
|
|
return UUtils::kInfinity;
|
|
}
|
|
|
|
|
|
//
|
|
// Inside
|
|
//
|
|
VUSolid::EnumInside UPolyhedraSide::Inside(const UVector3& p,
|
|
double tolerance,
|
|
double* bestDistance)
|
|
{
|
|
//
|
|
// Which phi segment is closest to this point?
|
|
//
|
|
int iPhi = ClosestPhiSegment(GetPhi(p));
|
|
|
|
double norm;
|
|
//
|
|
// Get distance to this segment
|
|
//
|
|
*bestDistance = DistanceToOneSide(p, vecs[iPhi], &norm);
|
|
|
|
//
|
|
// Use distance along normal to decide return value
|
|
//
|
|
if ((std::fabs(norm) < tolerance) && (*bestDistance < 2.0 * tolerance))
|
|
return VUSolid::eSurface;
|
|
|
|
if (norm < 0) return VUSolid::eInside;
|
|
|
|
return VUSolid::eOutside;
|
|
}
|
|
|
|
|
|
//
|
|
// Normal
|
|
//
|
|
UVector3 UPolyhedraSide::Normal(const UVector3& p,
|
|
double* bestDistance)
|
|
{
|
|
//
|
|
// Which phi segment is closest to this point?
|
|
//
|
|
int iPhi = ClosestPhiSegment(GetPhi(p));
|
|
|
|
//
|
|
// Get distance to this segment
|
|
//
|
|
double norm;
|
|
*bestDistance = DistanceToOneSide(p, vecs[iPhi], &norm);
|
|
|
|
return vecs[iPhi].normal;
|
|
}
|
|
|
|
|
|
//
|
|
// Extent
|
|
//
|
|
double UPolyhedraSide::Extent(const UVector3 axis)
|
|
{
|
|
if (axis.Perp2() < DBL_MIN)
|
|
{
|
|
//
|
|
// Special case
|
|
//
|
|
return axis.z < 0 ? -cone->ZLo() : cone->ZHi();
|
|
}
|
|
|
|
int iPhi, i1, i2;
|
|
double best;
|
|
UVector3* list[4];
|
|
|
|
//
|
|
// Which phi segment, if any, does the axis belong to
|
|
//
|
|
iPhi = PhiSegment(GetPhi(axis));
|
|
|
|
if (iPhi < 0)
|
|
{
|
|
//
|
|
// No phi segment? Check front edge of first side and
|
|
// last edge of second side
|
|
//
|
|
i1 = 0;
|
|
i2 = numSide - 1;
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// Check all corners of matching phi side
|
|
//
|
|
i1 = iPhi;
|
|
i2 = iPhi;
|
|
}
|
|
|
|
list[0] = vecs[i1].edges[0]->corner;
|
|
list[1] = vecs[i1].edges[0]->corner + 1;
|
|
list[2] = vecs[i2].edges[1]->corner;
|
|
list[3] = vecs[i2].edges[1]->corner + 1;
|
|
|
|
//
|
|
// Who's biggest?
|
|
//
|
|
best = -UUtils::kInfinity;
|
|
UVector3** vec = list;
|
|
do
|
|
{
|
|
double answer = (*vec)->Dot(axis);
|
|
if (answer > best) best = answer;
|
|
}
|
|
while (++vec < list + 4);
|
|
|
|
return best;
|
|
}
|
|
|
|
|
|
|
|
//
|
|
// IntersectSidePlane
|
|
//
|
|
// Decide if a line correctly intersects one side plane of our segment.
|
|
// It is assumed that the correct side has been chosen, and thus only
|
|
// the z bounds (of the entire segment) are checked.
|
|
//
|
|
// normSign - To be multiplied against normal:
|
|
// = +1.0 normal is unchanged
|
|
// = -1.0 normal is reversed (now points inward)
|
|
//
|
|
// Arguments:
|
|
// p - (in) Point
|
|
// v - (in) Direction
|
|
// vec - (in) Description record of the side plane
|
|
// normSign - (in) Sign (+/- 1) to apply to normal
|
|
// surfTolerance - (in) Surface tolerance (generally > 0, see below)
|
|
// distance - (out) Distance along v to intersection
|
|
// distFromSurface - (out) Distance from surface normal
|
|
//
|
|
// Notes:
|
|
// surfTolerance - Used to decide if a point is behind the surface,
|
|
// a point is allow to be -surfTolerance behind the
|
|
// surface (as measured along the normal), but *only*
|
|
// if the point is within the r/z bounds + surfTolerance
|
|
// of the segment.
|
|
//
|
|
bool UPolyhedraSide::IntersectSidePlane(const UVector3& p,
|
|
const UVector3& v,
|
|
const UPolyhedraSideVec& vec,
|
|
double normSign,
|
|
double surfTolerance,
|
|
double& distance,
|
|
double& distFromSurface)
|
|
{
|
|
//
|
|
// Correct normal? Here we have straight sides, and can safely ignore
|
|
// intersections where the Dot product with the normal is zero.
|
|
//
|
|
double dotProd = normSign * v.Dot(vec.normal);
|
|
|
|
if (dotProd <= 0) return false;
|
|
|
|
//
|
|
// Calculate distance to surface. If the side is too far
|
|
// behind the point, we must reject it.
|
|
//
|
|
UVector3 delta = p - vec.center;
|
|
distFromSurface = -normSign * delta.Dot(vec.normal);
|
|
|
|
if (distFromSurface < -surfTolerance) return false;
|
|
|
|
//
|
|
// Calculate precise distance to intersection with the side
|
|
// (along the trajectory, not normal to the surface)
|
|
//
|
|
distance = distFromSurface / dotProd;
|
|
|
|
//
|
|
// Do we fall off the r/z extent of the segment?
|
|
//
|
|
// Calculate this very, very carefully! Why?
|
|
// 1. If a RZ end is at R=0, you can't miss!
|
|
// 2. If you just fall off in RZ, the answer must
|
|
// be consistent with adjacent UPolyhedraSide faces.
|
|
// (2) implies that only variables used by other UPolyhedraSide
|
|
// faces may be used, which includes only: p, v, and the edge corners.
|
|
// It also means that one side is a ">" or "<", which the other
|
|
// must be ">=" or "<=". Fortunately, this isn't a new problem.
|
|
// The solution below I borrowed from Joseph O'Rourke,
|
|
// "Computational Geometry in C (Second Edition)"
|
|
// See: http://cs.smith.edu/~orourke/
|
|
//
|
|
UVector3 ic = p + distance * v - vec.center;
|
|
double atRZ = vec.surfRZ.Dot(ic);
|
|
|
|
if (atRZ < 0)
|
|
{
|
|
if (r[0] == 0) return true; // Can't miss!
|
|
|
|
if (atRZ < -lenRZ * 1.2) return false; // Forget it! Missed by a mile.
|
|
|
|
UVector3 q = p + v;
|
|
UVector3 qa = q - vec.edges[0]->corner[0],
|
|
qb = q - vec.edges[1]->corner[0];
|
|
UVector3 qacb = qa.Cross(qb);
|
|
if (normSign * qacb.Dot(v) < 0) return false;
|
|
|
|
if (distFromSurface < 0)
|
|
{
|
|
if (atRZ < -lenRZ - surfTolerance) return false;
|
|
}
|
|
}
|
|
else if (atRZ > 0)
|
|
{
|
|
if (r[1] == 0) return true; // Can't miss!
|
|
|
|
if (atRZ > lenRZ * 1.2) return false; // Missed by a mile
|
|
|
|
UVector3 q = p + v;
|
|
UVector3 qa = q - vec.edges[0]->corner[1],
|
|
qb = q - vec.edges[1]->corner[1];
|
|
UVector3 qacb = qa.Cross(qb);
|
|
if (normSign * qacb.Dot(v) >= 0) return false;
|
|
|
|
if (distFromSurface < 0)
|
|
{
|
|
if (atRZ > lenRZ + surfTolerance) return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
//
|
|
// LineHitsSegments
|
|
//
|
|
// Calculate which phi segments a line intersects in three dimensions.
|
|
// No check is made as to whether the intersections are within the z bounds of
|
|
// the segment.
|
|
//
|
|
int UPolyhedraSide::LineHitsSegments(const UVector3& p,
|
|
const UVector3& v,
|
|
int* i1, int* i2)
|
|
{
|
|
double s1, s2;
|
|
//
|
|
// First, decide if and where the line intersects the cone
|
|
//
|
|
int n = cone->LineHitsCone(p, v, s1, s2);
|
|
|
|
if (n == 0) return 0;
|
|
|
|
//
|
|
// Try first intersection.
|
|
//
|
|
*i1 = PhiSegment(std::atan2(p.y + s1 * v.y, p.x + s1 * v.x));
|
|
if (n == 1)
|
|
{
|
|
return (*i1 < 0) ? 0 : 1;
|
|
}
|
|
|
|
//
|
|
// Try second intersection
|
|
//
|
|
*i2 = PhiSegment(std::atan2(p.y + s2 * v.y, p.x + s2 * v.x));
|
|
if (*i1 == *i2) return 0;
|
|
|
|
if (*i1 < 0)
|
|
{
|
|
if (*i2 < 0) return 0;
|
|
*i1 = *i2;
|
|
return 1;
|
|
}
|
|
|
|
if (*i2 < 0) return 1;
|
|
|
|
return 2;
|
|
}
|
|
|
|
|
|
//
|
|
// ClosestPhiSegment
|
|
//
|
|
// Decide which phi segment is closest in phi to the point.
|
|
// The result is the same as PhiSegment if there is no phi opening.
|
|
//
|
|
int UPolyhedraSide::ClosestPhiSegment(double phi0)
|
|
{
|
|
int iPhi = PhiSegment(phi0);
|
|
if (iPhi >= 0) return iPhi;
|
|
|
|
//
|
|
// Boogers! The points falls inside the phi segment.
|
|
// Look for the closest point: the start, or end
|
|
//
|
|
double phi = phi0;
|
|
|
|
while (phi < startPhi) phi += 2 * UUtils::kPi;
|
|
double d1 = phi - endPhi;
|
|
|
|
while (phi > startPhi) phi -= 2 * UUtils::kPi;
|
|
double d2 = startPhi - phi;
|
|
|
|
return (d2 < d1) ? 0 : numSide - 1;
|
|
}
|
|
|
|
|
|
//
|
|
// PhiSegment
|
|
//
|
|
// Decide which phi segment an angle belongs to, counting from zero.
|
|
// A value of -1 indicates that the phi value is outside the shape
|
|
// (only possible if phiTotal < 360 degrees).
|
|
//
|
|
int UPolyhedraSide::PhiSegment(double phi0)
|
|
{
|
|
//
|
|
// How far are we from phiStart? Come up with a positive answer
|
|
// that is less than 2*PI
|
|
//
|
|
double phi = phi0 - startPhi;
|
|
while (phi < 0) phi += 2 * UUtils::kPi;
|
|
while (phi > 2 * UUtils::kPi) phi -= 2 * UUtils::kPi;
|
|
|
|
//
|
|
// Divide
|
|
//
|
|
int answer = (int)(phi / deltaPhi);
|
|
|
|
if (answer >= numSide)
|
|
{
|
|
if (phiIsOpen)
|
|
return -1; // Looks like we missed
|
|
else
|
|
answer = numSide - 1; // Probably just roundoff
|
|
}
|
|
|
|
return answer;
|
|
}
|
|
|
|
|
|
//
|
|
// GetPhi
|
|
//
|
|
// Calculate Phi for a given 3-vector (point), if not already cached for the
|
|
// same point, in the attempt to avoid consecutive computation of the same
|
|
// quantity
|
|
//
|
|
double UPolyhedraSide::GetPhi(const UVector3& p)
|
|
{
|
|
double val = 0.;
|
|
|
|
if (fPhi.first != p)
|
|
{
|
|
val = p.Phi();
|
|
fPhi.first = p;
|
|
fPhi.second = val;
|
|
}
|
|
else
|
|
{
|
|
val = fPhi.second;
|
|
}
|
|
return val;
|
|
}
|
|
|
|
|
|
//
|
|
// DistanceToOneSide
|
|
//
|
|
// Arguments:
|
|
// p - (in) Point to check
|
|
// vec - (in) vector Set of this side
|
|
// normDist - (out) distance normal to the side or edge, as appropriate, signed
|
|
// Return value = total distance from the side
|
|
//
|
|
double UPolyhedraSide::DistanceToOneSide(const UVector3& p,
|
|
const UPolyhedraSideVec& vec,
|
|
double* normDist)
|
|
{
|
|
UVector3 pct = p - vec.center;
|
|
|
|
//
|
|
// Get normal distance
|
|
//
|
|
*normDist = vec.normal.Dot(pct);
|
|
|
|
//
|
|
// Add edge penalty
|
|
//
|
|
return DistanceAway(p, vec, normDist);
|
|
}
|
|
|
|
|
|
//
|
|
// DistanceAway
|
|
//
|
|
// Add distance from side edges, if necesssary, to total distance,
|
|
// and updates normDist appropriate depending on edge normals.
|
|
//
|
|
double UPolyhedraSide::DistanceAway(const UVector3& p,
|
|
const UPolyhedraSideVec& vec,
|
|
double* normDist)
|
|
{
|
|
double distOut2;
|
|
UVector3 pct = p - vec.center;
|
|
double distFaceNorm = *normDist;
|
|
//
|
|
// Okay, are we inside bounds?
|
|
//
|
|
double pcDotRZ = pct.Dot(vec.surfRZ);
|
|
double pcDotPhi = pct.Dot(vec.surfPhi);
|
|
|
|
//
|
|
// Go through all permutations.
|
|
// Phi
|
|
// | | ^
|
|
// B | H | E |
|
|
// ------[1]------------[3]----- |
|
|
// |XXXXXXXXXXXXXX| +----> RZ
|
|
// C |XXXXXXXXXXXXXX| F
|
|
// |XXXXXXXXXXXXXX|
|
|
// ------[0]------------[2]----
|
|
// A | G | D
|
|
// | |
|
|
//
|
|
// It's real messy, but at least it's quick
|
|
//
|
|
|
|
if (pcDotRZ < -lenRZ)
|
|
{
|
|
double lenPhiZ = lenPhi[0] - lenRZ * lenPhi[1];
|
|
double distOutZ = pcDotRZ + lenRZ;
|
|
//
|
|
// Below in RZ
|
|
//
|
|
if (pcDotPhi < -lenPhiZ)
|
|
{
|
|
//
|
|
// ...and below in phi. Find distance to point (A)
|
|
//
|
|
double distOutPhi = pcDotPhi + lenPhiZ;
|
|
distOut2 = distOutPhi * distOutPhi + distOutZ * distOutZ;
|
|
UVector3 pa = p - vec.edges[0]->corner[0];
|
|
*normDist = pa.Dot(vec.edges[0]->cornNorm[0]);
|
|
}
|
|
else if (pcDotPhi > lenPhiZ)
|
|
{
|
|
//
|
|
// ...and above in phi. Find distance to point (B)
|
|
//
|
|
double distOutPhi = pcDotPhi - lenPhiZ;
|
|
distOut2 = distOutPhi * distOutPhi + distOutZ * distOutZ;
|
|
UVector3 pb = p - vec.edges[1]->corner[0];
|
|
*normDist = pb.Dot(vec.edges[1]->cornNorm[0]);
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// ...and inside in phi. Find distance to line (C)
|
|
//
|
|
UVector3 pa = p - vec.edges[0]->corner[0];
|
|
distOut2 = distOutZ * distOutZ;
|
|
*normDist = pa.Dot(vec.edgeNorm[0]);
|
|
}
|
|
}
|
|
else if (pcDotRZ > lenRZ)
|
|
{
|
|
double lenPhiZ = lenPhi[0] + lenRZ * lenPhi[1];
|
|
double distOutZ = pcDotRZ - lenRZ;
|
|
//
|
|
// Above in RZ
|
|
//
|
|
if (pcDotPhi < -lenPhiZ)
|
|
{
|
|
//
|
|
// ...and below in phi. Find distance to point (D)
|
|
//
|
|
double distOutPhi = pcDotPhi + lenPhiZ;
|
|
distOut2 = distOutPhi * distOutPhi + distOutZ * distOutZ;
|
|
UVector3 pd = p - vec.edges[0]->corner[1];
|
|
*normDist = pd.Dot(vec.edges[0]->cornNorm[1]);
|
|
}
|
|
else if (pcDotPhi > lenPhiZ)
|
|
{
|
|
//
|
|
// ...and above in phi. Find distance to point (E)
|
|
//
|
|
double distOutPhi = pcDotPhi - lenPhiZ;
|
|
distOut2 = distOutPhi * distOutPhi + distOutZ * distOutZ;
|
|
UVector3 pe = p - vec.edges[1]->corner[1];
|
|
*normDist = pe.Dot(vec.edges[1]->cornNorm[1]);
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// ...and inside in phi. Find distance to line (F)
|
|
//
|
|
distOut2 = distOutZ * distOutZ;
|
|
UVector3 pd = p - vec.edges[0]->corner[1];
|
|
*normDist = pd.Dot(vec.edgeNorm[1]);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
double lenPhiZ = lenPhi[0] + pcDotRZ * lenPhi[1];
|
|
//
|
|
// We are inside RZ bounds
|
|
//
|
|
if (pcDotPhi < -lenPhiZ)
|
|
{
|
|
//
|
|
// ...and below in phi. Find distance to line (G)
|
|
//
|
|
double distOut = edgeNorm * (pcDotPhi + lenPhiZ);
|
|
distOut2 = distOut * distOut;
|
|
UVector3 pd = p - vec.edges[0]->corner[1];
|
|
*normDist = pd.Dot(vec.edges[0]->normal);
|
|
}
|
|
else if (pcDotPhi > lenPhiZ)
|
|
{
|
|
//
|
|
// ...and above in phi. Find distance to line (H)
|
|
//
|
|
double distOut = edgeNorm * (pcDotPhi - lenPhiZ);
|
|
distOut2 = distOut * distOut;
|
|
UVector3 pe = p - vec.edges[1]->corner[1];
|
|
*normDist = pe.Dot(vec.edges[1]->normal);
|
|
}
|
|
else
|
|
{
|
|
//
|
|
// Inside bounds! No penalty.
|
|
//
|
|
return std::fabs(distFaceNorm);
|
|
}
|
|
}
|
|
return std::sqrt(distFaceNorm * distFaceNorm + distOut2);
|
|
}
|
|
|
|
|
|
//
|
|
// Calculation of surface area of a triangle.
|
|
// At the same time a random point in the triangle is given
|
|
//
|
|
double UPolyhedraSide::SurfaceTriangle(UVector3 p1,
|
|
UVector3 p2,
|
|
UVector3 p3,
|
|
UVector3* p4)
|
|
{
|
|
UVector3 v, w;
|
|
|
|
v = p3 - p1;
|
|
w = p1 - p2;
|
|
double lambda1 = UUtils::Random();
|
|
double lambda2 = lambda1 * UUtils::Random();
|
|
|
|
*p4 = p2 + lambda1 * w + lambda2 * v;
|
|
return 0.5 * (v.Cross(w)).Mag();
|
|
}
|
|
|
|
|
|
//
|
|
// GetPointOnPlane
|
|
//
|
|
// Auxiliary method for GetPointOnSurface()
|
|
//
|
|
UVector3
|
|
UPolyhedraSide::GetPointOnPlane(UVector3 p0, UVector3 p1,
|
|
UVector3 p2, UVector3 p3,
|
|
double* Area)
|
|
{
|
|
double chose, aOne, aTwo;
|
|
UVector3 point1, point2;
|
|
|
|
aOne = SurfaceTriangle(p0, p1, p2, &point1);
|
|
aTwo = SurfaceTriangle(p2, p3, p0, &point2);
|
|
*Area = aOne + aTwo;
|
|
|
|
chose = UUtils::Random() * (aOne + aTwo);
|
|
if ((chose >= 0.) && (chose < aOne))
|
|
{
|
|
return (point1);
|
|
}
|
|
return (point2);
|
|
}
|
|
|
|
|
|
//
|
|
// SurfaceArea()
|
|
//
|
|
double UPolyhedraSide::SurfaceArea()
|
|
{
|
|
if (fSurfaceArea == 0.)
|
|
{
|
|
// Define the variables
|
|
//
|
|
double area, areas;
|
|
UVector3 point1;
|
|
UVector3 v1, v2, v3, v4;
|
|
UPolyhedraSideVec* vec = vecs;
|
|
areas = 0.;
|
|
|
|
// Do a loop on all SideEdge
|
|
//
|
|
do
|
|
{
|
|
// Define 4points for a Plane or Triangle
|
|
//
|
|
v1 = vec->edges[0]->corner[0];
|
|
v2 = vec->edges[0]->corner[1];
|
|
v3 = vec->edges[1]->corner[1];
|
|
v4 = vec->edges[1]->corner[0];
|
|
point1 = GetPointOnPlane(v1, v2, v3, v4, &area);
|
|
areas += area;
|
|
}
|
|
while (++vec < vecs + numSide);
|
|
|
|
fSurfaceArea = areas;
|
|
}
|
|
return fSurfaceArea;
|
|
}
|
|
|
|
|
|
//
|
|
// GetPointOnFace()
|
|
//
|
|
UVector3 UPolyhedraSide::GetPointOnFace()
|
|
{
|
|
// Define the variables
|
|
//
|
|
std::vector<double> areas;
|
|
std::vector<UVector3> points;
|
|
double area = 0;
|
|
double result1;
|
|
UVector3 point1;
|
|
UVector3 v1, v2, v3, v4;
|
|
UPolyhedraSideVec* vec = vecs;
|
|
|
|
// Do a loop on all SideEdge
|
|
//
|
|
do
|
|
{
|
|
// Define 4points for a Plane or Triangle
|
|
//
|
|
v1 = vec->edges[0]->corner[0];
|
|
v2 = vec->edges[0]->corner[1];
|
|
v3 = vec->edges[1]->corner[1];
|
|
v4 = vec->edges[1]->corner[0];
|
|
point1 = GetPointOnPlane(v1, v2, v3, v4, &result1);
|
|
points.push_back(point1);
|
|
areas.push_back(result1);
|
|
area += result1;
|
|
}
|
|
while (++vec < vecs + numSide);
|
|
|
|
// Choose randomly one of the surfaces and point on it
|
|
//
|
|
double chose = area * UUtils::Random();
|
|
double Achose1, Achose2;
|
|
Achose1 = 0;
|
|
Achose2 = 0.;
|
|
int i = 0;
|
|
do
|
|
{
|
|
Achose2 += areas[i];
|
|
if (chose >= Achose1 && chose < Achose2)
|
|
{
|
|
point1 = points[i] ;
|
|
break;
|
|
}
|
|
i++;
|
|
Achose1 = Achose2;
|
|
}
|
|
while (i < numSide);
|
|
|
|
return point1;
|
|
}
|