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;
}
void rms::FitAABox( const Wml::Vector3<Real> & vCenter, Real fRadius, Wml::AxisAlignedBox3<Real> & aaBox )
{
aaBox.Min[0] = vCenter.X() - fRadius;
aaBox.Max[0] = vCenter.X() + fRadius;
aaBox.Min[1] = vCenter.Y() - fRadius;
aaBox.Max[1] = vCenter.Y() + fRadius;
aaBox.Min[2] = vCenter.Z() - fRadius;
aaBox.Max[2] = vCenter.Z() + fRadius;
}
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();
}
}
void PolyLoop3<Real>::Scale( Real fScaleX, Real fScaleY, Real fScaleZ, const Wml::Vector3<Real> & vOrigin )
{
size_t nCount = m_vVertices.size();
for (unsigned int i = 0; i < nCount; ++i) {
m_vVertices[i].X() = (m_vVertices[i].X() - vOrigin.X()) * fScaleX + vOrigin.X();
m_vVertices[i].Y() = (m_vVertices[i].Y() - vOrigin.Y()) * fScaleY + vOrigin.Y();
m_vVertices[i].Z() = (m_vVertices[i].Z() - vOrigin.Z()) * fScaleZ + vOrigin.Z();
}
}
Wml::Quaternion<Real> fromExponentialMap(const Wml::Vector3<Real>& v, bool reparam)
{
Real theta = v.Length();
if(reparam)
{
if(theta > +Wml::Math<Real>::PI) { theta = (theta - Wml::Math<Real>::TWO_PI); }
if(theta < -Wml::Math<Real>::PI) { theta = (Wml::Math<Real>::TWO_PI + theta); }
}
Real halftheta = (Real)0.5 * theta;
Real c = (theta > Wml::Math<Real>::EPSILON) ? ( (Real)sin(halftheta) / theta ) : ( (Real)0.5 + (theta*theta)/(Real)48.0 );
float fTuple[4];
fTuple[0] = (Real)cos(halftheta);
for(int i=0; i<3; i++)
{
fTuple[i+1] = c * v[i];
}
return Wml::Quaternion<Real>(fTuple[0], fTuple[1], fTuple[2], fTuple[3]);
}
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);
}
Wml::Vector3<Real> rms::NCross( const Wml::Vector3<Real> & v1, const Wml::Vector3<Real> & v2 )
{
Wml::Vector3<Real> n(v1.Cross(v2));
n.Normalize();
return n;
}