Real ExtPlane3<Real>::IntersectRay( const Wml::Vector3<Real> & vOrigin,
const Wml::Vector3<Real> & vDirection ) const
{
Wml::Vector3<Real> vN(this->Normal);
Real fDenom = vDirection.Dot(vN);
if ( fDenom == 0 )
return std::numeric_limits<Real>::max();
return (this->Constant - vOrigin.Dot(vN)) / fDenom;
}
static void
simplifyDP( Real tol, std::vector<Wml::Vector3<Real> > & v, int j, int k, std::vector<bool> & mk )
{
if (k <= j+1) // there is nothing to simplify
return;
// check for adequate approximation by segment S from v[j] to v[k]
int maxi = j; // index of vertex farthest from S
Real maxd2 = 0; // distance squared of farthest vertex
Real tol2 = tol * tol; // tolerance squared
Wml::Segment3<Real> S; // segment from v[j] to v[k]
S.Origin = v[j];
S.Direction = v[k] - v[j];
S.Extent = S.Direction.Normalize();
Wml::Vector3<Real> u( S.Direction ); // segment direction vector
Real cu = u.SquaredLength(); // segment length squared
// test each vertex v[i] for max distance from S
// compute using the Feb 2001 Algorithm's dist_Point_to_Segment()
// Note: this works in any dimension (2D, 3D, ...)
Wml::Vector3<Real> w;
Wml::Vector3<Real> Pb; // base of perpendicular from v[i] to S
Real b, cw, dv2; // dv2 = distance v[i] to S squared
for (int i = j+1; i < k; i++)
{
// compute distance squared
w = v[i] - S.Origin;
cw = w.Dot(u);
if ( cw <= 0 )
dv2 = (v[i] - S.Origin).SquaredLength();
else if ( cu <= cw )
dv2 = (v[i] - (S.Origin + S.Direction)).SquaredLength();
else {
b = cw / cu;
Pb = S.Origin + u * b;
dv2 = (v[i] - Pb).SquaredLength();
}
// test with current max distance squared
if (dv2 <= maxd2)
continue;
// v[i] is a new max vertex
maxi = i;
maxd2 = dv2;
}
if (maxd2 > tol2) // error is worse than the tolerance
{
// split the polyline at the farthest vertex from S
mk[maxi] = true; // mark v[maxi] for the simplified polyline
// recursively simplify the two subpolylines at v[maxi]
simplifyDP( tol, v, j, maxi, mk ); // polyline v[j] to v[maxi]
simplifyDP( tol, v, maxi, k, mk ); // polyline v[maxi] to v[k]
}
// else the approximation is OK, so ignore intermediate vertices
return;
}
void Wml::StretchMetric1( const Vector3<Real> & q1,
const Vector3<Real> & q2,
const Vector3<Real> & q3,
const Vector2<Real> & p1,
const Vector2<Real> & p2,
const Vector2<Real> & p3,
Real & MaxSV, Real & MinSV, Real & L2Norm, Real & LInfNorm )
{
Real s1 = p1.X();
Real t1 = p1.Y();
Real s2 = p2.X();
Real t2 = p2.Y();
Real s3 = p3.X();
Real t3 = p3.Y();
Real A = (Real)0.5 * ( (s2 - s1) * (t3 - t1) - (s3 - s1) * (t2 - t1));
if ( A > 0 ) {
Wml::Vector3<Real> Ss =
(q1 * (t2-t3) + q2 * (t3-t1) + q3 * (t1-t2)) / (2*A);
Wml::Vector3<Real> St =
(q1 * (s3-s2) + q2 * (s1-s3) + q3 * (s2-s1)) / (2*A);
Real a = Ss.Dot(Ss);
Real b = Ss.Dot(St);
Real c = St.Dot(St);
Real discrim = (Real)sqrt( (a-c)*(a-c) + 4*b*b );
MaxSV = (Real)sqrt( (Real)0.5 * ( (a+c) + discrim ) );
MinSV = (Real)sqrt( (Real)0.5 * ( (a+c) - discrim ) );
L2Norm = (Real)sqrt( (Real)0.5 * (a+c) );
LInfNorm = MaxSV;
} else {
MaxSV = MinSV = L2Norm = LInfNorm = std::numeric_limits<Real>::max();
}
}
Real rms::VectorCot( const Wml::Vector3<Real> & v1, const Wml::Vector3<Real> & v2 )
{
Real fDot = v1.Dot(v2);
return fDot / (Real)sqrt( v1.Dot(v1) * v2.Dot(v2) - fDot*fDot );
}
Real rms::VectorAngle( const Wml::Vector3<Real> & v1, const Wml::Vector3<Real> & v2 )
{
Real fDot = Clamp(v1.Dot(v2), (Real)-1.0, (Real)1.0);
return (Real)acos(fDot);
}