//----------------------------------------------------------------------------
Vector3 Curve3::GetBinormal (Real fTime) const
{
Vector3 kVelocity = GetFirstDerivative(fTime);
Vector3 kAcceleration = GetSecondDerivative(fTime);
Real fVDotV = kVelocity.Dot(kVelocity);
Real fVDotA = kVelocity.Dot(kAcceleration);
Vector3 kNormal = fVDotV*kAcceleration - fVDotA*kVelocity;
kNormal.Unitize();
kVelocity.Unitize();
Vector3 kBinormal = kVelocity.Cross(kNormal);
return kBinormal;
}
//----------------------------------------------------------------------------
void Curve3::GetFrame (Real fTime, Vector3& rkPosition, Vector3& rkTangent,
Vector3& rkNormal, Vector3& rkBinormal) const
{
rkPosition = GetPosition(fTime);
Vector3 kVelocity = GetFirstDerivative(fTime);
Vector3 kAcceleration = GetSecondDerivative(fTime);
Real fVDotV = kVelocity.Dot(kVelocity);
Real fVDotA = kVelocity.Dot(kAcceleration);
rkNormal = fVDotV*kAcceleration - fVDotA*kVelocity;
rkNormal.Unitize();
rkTangent = kVelocity;
rkTangent.Unitize();
rkBinormal = rkTangent.Cross(rkNormal);
}
//----------------------------------------------------------------------------
void ConvexPolyhedron::Create (const vector<Vector3>& rakPoint,
const vector<int>& raiConnect)
{
assert( rakPoint.size() >= 4 && raiConnect.size() >= 4 );
int iVQuantity = rakPoint.size();
int iTQuantity = raiConnect.size()/3;
int iEQuantity = iVQuantity + iTQuantity - 2;
Reset(iVQuantity,iEQuantity,iTQuantity);
m_akPoint = rakPoint;
// Copy polyhedron points into vertex array. Compute centroid for use in
// making sure the triangles are counterclockwise oriented when viewed
// from the outside.
ComputeCentroid();
// get polyhedron edge and triangle information
for (int iT = 0, iIndex = 0; iT < iTQuantity; iT++)
{
// get vertex indices for triangle
int iV0 = raiConnect[iIndex++];
int iV1 = raiConnect[iIndex++];
int iV2 = raiConnect[iIndex++];
// make sure triangle is counterclockwise
Vector3& rkV0 = m_akPoint[iV0];
Vector3& rkV1 = m_akPoint[iV1];
Vector3& rkV2 = m_akPoint[iV2];
Vector3 kDiff = m_kCentroid - rkV0;
Vector3 kE1 = rkV1 - rkV0;
Vector3 kE2 = rkV2 - rkV0;
Vector3 kNormal = kE1.Cross(kE2);
Real fLength = kNormal.Length();
if ( fLength > 1e-06f )
{
kNormal /= fLength;
}
else
{
kNormal = kDiff;
kNormal.Unitize();
}
Real fDistance = kNormal.Dot(kDiff);
if ( fDistance < 0.0f )
{
// triangle is counterclockwise
Insert(iV0,iV1,iV2);
}
else
{
// triangle is clockwise
Insert(iV0,iV2,iV1);
}
}
UpdatePlanes();
}
//----------------------------------------------------------------------------
bool Mgc::Culled (const Plane& rkPlane, const Sphere& rkSphere,
bool bUnitNormal)
{
Vector3 kNormal = rkPlane.Normal();
Real fConstant = rkPlane.Constant();
if ( !bUnitNormal )
{
Real fLength = kNormal.Unitize();
fConstant /= fLength;
}
Real fTmp = kNormal.Dot(rkSphere.Center()) - fConstant;
return fTmp <= -rkSphere.Radius();
}
//----------------------------------------------------------------------------
bool Mgc::TestIntersection (const Plane& rkPlane, const Sphere& rkSphere,
bool bUnitNormal)
{
Vector3 kNormal = rkPlane.Normal();
Real fConstant = rkPlane.Constant();
if ( !bUnitNormal )
{
Real fLength = kNormal.Unitize();
fConstant /= fLength;
}
Real fPseudoDistance = kNormal.Dot(rkSphere.Center()) - fConstant;
return Math::FAbs(fPseudoDistance) <= rkSphere.Radius();
}
//----------------------------------------------------------------------------
bool Mgc::Culled (const Plane& rkPlane, const Ellipsoid& rkEllipsoid,
bool bUnitNormal)
{
Vector3 kNormal = rkPlane.Normal();
Real fConstant = rkPlane.Constant();
if ( !bUnitNormal )
{
Real fLength = kNormal.Unitize();
fConstant /= fLength;
}
Real fDiscr = kNormal.Dot(rkEllipsoid.InverseA()*kNormal);
Real fRoot = Math::Sqrt(Math::FAbs(fDiscr));
Real fSDist = kNormal.Dot(rkEllipsoid.Center()) - fConstant;
return fSDist <= -fRoot;
}
//----------------------------------------------------------------------------
bool Mgc::Culled (const Plane& rkPlane, const Lozenge& rkLozenge,
bool bUnitNormal)
{
Vector3 kNormal = rkPlane.Normal();
Real fConstant = rkPlane.Constant();
if ( !bUnitNormal )
{
Real fLength = kNormal.Unitize();
fConstant /= fLength;
}
Real fTmp00 = kNormal.Dot(rkLozenge.Origin()) - fConstant;
if ( fTmp00 < 0.0f )
{
Real fDotNE0 = kNormal.Dot(rkLozenge.Edge0());
Real fTmp10 = fTmp00 + fDotNE0;
if ( fTmp10 < 0.0f )
{
Real fDotNE1 = kNormal.Dot(rkLozenge.Edge1());
Real fTmp01 = fTmp00 + fDotNE1;
if ( fTmp01 < 0.0f )
{
Real fTmp11 = fTmp10 + fDotNE1;
if ( fTmp11 < 0.0f )
{
// all four lozenge corners on negative side of plane
if ( fTmp00 <= fTmp10 )
{
if ( fTmp00 <= fTmp01 )
return fTmp00 <= -rkLozenge.Radius();
else
return fTmp01 <= -rkLozenge.Radius();
}
else
{
if ( fTmp10 <= fTmp11 )
return fTmp10 <= -rkLozenge.Radius();
else
return fTmp11 <= -rkLozenge.Radius();
}
}
}
}
}
return false;
}
//----------------------------------------------------------------------------
void ConvexPolyhedron::UpdatePlanes ()
{
// The planes are constructed to have *inner pointing* normals. This
// supports my Wild Magic software clipping code that was based on a view
// frustum having inner pointing normals.
ComputeCentroid();
int iTQuantity = m_akTriangle.GetQuantity();
m_akPlane.resize(iTQuantity);
for (int iT = 0; iT < iTQuantity; iT++)
{
MTTriangle& rkT = m_akTriangle[iT];
int iV0 = GetVLabel(rkT.Vertex(0));
int iV1 = GetVLabel(rkT.Vertex(1));
int iV2 = GetVLabel(rkT.Vertex(2));
Vector3& rkV0 = m_akPoint[iV0];
Vector3& rkV1 = m_akPoint[iV1];
Vector3& rkV2 = m_akPoint[iV2];
Vector3 kDiff = m_kCentroid - rkV0;
Vector3 kE1 = rkV1 - rkV0;
Vector3 kE2 = rkV2 - rkV0;
Vector3 kNormal = kE2.Cross(kE1);
Real fLength = kNormal.Length();
if ( fLength > 1e-06f )
{
kNormal /= fLength;
Real fDot = kNormal.Dot(kDiff);
if ( fDot < 0.0f )
kNormal = -kNormal;
}
else
{
// triangle is degenerate, use "normal" that points towards
// centroid
kNormal = kDiff;
kNormal.Unitize();
}
// inner pointing normal
m_akPlane[iT] = Plane(kNormal,kNormal.Dot(rkV0));
}
}
//----------------------------------------------------------------------------
bool Mgc::TestIntersection (const Plane& rkPlane, const Lozenge& rkLozenge,
bool bUnitNormal)
{
Vector3 kNormal = rkPlane.Normal();
Real fConstant = rkPlane.Constant();
if ( !bUnitNormal )
{
Real fLength = kNormal.Unitize();
fConstant /= fLength;
}
Vector3 kC10 = rkLozenge.Origin() + rkLozenge.Edge0();
Vector3 kC01 = rkLozenge.Origin() + rkLozenge.Edge1();
Vector3 kC11 = kC10 + rkLozenge.Edge1();
Real fTmp00 = kNormal.Dot(rkLozenge.Origin()) - fConstant;
Real fTmp10 = kNormal.Dot(kC10) - fConstant;
if ( fTmp00*fTmp10 <= 0.0f )
{
// two lozenge ends on opposite sides of the plane
return true;
}
Real fTmp01 = kNormal.Dot(kC01) - fConstant;
if ( fTmp00*fTmp01 <= 0.0f )
{
// two lozenge ends on opposite sides of the plane
return true;
}
Real fTmp11 = kNormal.Dot(kC11) - fConstant;
if ( fTmp10*fTmp11 <= 0.0f )
{
// two lozenge ends on opposite sides of the plane
return true;
}
return Math::FAbs(fTmp00) <= rkLozenge.Radius()
|| Math::FAbs(fTmp10) <= rkLozenge.Radius()
|| Math::FAbs(fTmp01) <= rkLozenge.Radius()
|| Math::FAbs(fTmp11) <= rkLozenge.Radius();
}
//----------------------------------------------------------------------------
static bool PointInsideCone (const Vector3& rkP, const Cone3& rkCone)
{
Vector3 kDiff = rkP - rkCone.Vertex();
kDiff.Unitize();
return rkCone.Axis().Dot(kDiff) >= rkCone.CosAngle();
}
//----------------------------------------------------------------------------
Vector3 Curve3::GetTangent (Real fTime) const
{
Vector3 kVelocity = GetFirstDerivative(fTime);
kVelocity.Unitize();
return kVelocity;
}