Vector3<Real> Curve3<Real>::GetNormal (Real t) const
{
Vector3<Real> velocity = GetFirstDerivative(t);
Vector3<Real> acceleration = GetSecondDerivative(t);
Real VDotV = velocity.Dot(velocity);
Real VDotA = velocity.Dot(acceleration);
Vector3<Real> normal = VDotV*acceleration - VDotA*velocity;
normal.Normalize();
return normal;
}
//----------------------------------------------------------------------------
Vector3 Curve3::GetNormal (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();
return kNormal;
}
//----------------------------------------------------------------------------
// @ ::DistanceSquared()
// ---------------------------------------------------------------------------
// Returns the distance squared between line and point.
//-----------------------------------------------------------------------------
float DistanceSquared( const Line3& line, const Vector3& point, float& t_c )
{
Vector3 w = point - line.mOrigin;
float vsq = line.mDirection.Dot(line.mDirection);
float proj = w.Dot(line.mDirection);
t_c = proj/vsq;
return w.Dot(w) - t_c*proj;
} // End of ::DistanceSquared()
bool LinePlaneIntersection(Vector3 linePoint, Vector3 lineDir, Vector3 planePoint, Vector3 planeNormal, float & t)
{
float nDotDir = planeNormal.Dot(lineDir);
if (nDotDir != 0.f) {
t = (planeNormal.Dot(planePoint) - planeNormal.Dot(linePoint)) / nDotDir;
return true;
}
// parallel
return false;
}
void ClosestPoints(Vector3 p, Vector3 pDir, Vector3 q, Vector3 qDir, float & pt, float & qt)
{
Vector3 w = p - q;
float a = p.LengthSquared();
float b = pDir.Dot(qDir);
float c = q.LengthSquared();
float d = pDir.Dot(w);
float e = qDir.Dot(w);
pt = (b * e - c * d) / (a * c - b * b);
qt = (a * e - b * d) / (a * c - b * b);
}
/******************************************************************************
vector projection
dir vector must be normalized for maximum accuracy
******************************************************************************/
Vector3 Line::vectorProjection(Vector3& projected, Vector3& dir)
{
Vector3 part1;
part1.x = projected.Dot(dir);
part1.y = dir.Dot(dir);
Vector3 returnVec;
returnVec.x = (part1.x * dir.x) / part1.y;
returnVec.y = (part1.x * dir.y) / part1.y;
return returnVec;
}
void Wml::BoxProjection (const Vector3<Real>& rkAxis,
const Box3<Real>& rkBox, Real& rfMin, Real& rfMax)
{
Real fOrigin = rkAxis.Dot(rkBox.Center());
Real fMaximumExtent =
Math<Real>::FAbs(rkBox.Extent(0)*rkAxis.Dot(rkBox.Axis(0))) +
Math<Real>::FAbs(rkBox.Extent(1)*rkAxis.Dot(rkBox.Axis(1))) +
Math<Real>::FAbs(rkBox.Extent(2)*rkAxis.Dot(rkBox.Axis(2)));
rfMin = fOrigin - fMaximumExtent;
rfMax = fOrigin + fMaximumExtent;
}
Box3<Real> Wml::ContOrientedBox (int iQuantity, const Vector3<Real>* akPoint)
{
Box3<Real> kBox;
GaussPointsFit(iQuantity,akPoint,kBox.Center(),kBox.Axes(),
kBox.Extents());
// Let C be the box center and let U0, U1, and U2 be the box axes. Each
// input point is of the form X = C + y0*U0 + y1*U1 + y2*U2. The
// following code computes min(y0), max(y0), min(y1), max(y1), min(y2),
// and max(y2). The box center is then adjusted to be
// C' = C + 0.5*(min(y0)+max(y0))*U0 + 0.5*(min(y1)+max(y1))*U1 +
// 0.5*(min(y2)+max(y2))*U2
Vector3<Real> kDiff = akPoint[0] - kBox.Center();
Real fY0Min = kDiff.Dot(kBox.Axis(0)), fY0Max = fY0Min;
Real fY1Min = kDiff.Dot(kBox.Axis(1)), fY1Max = fY1Min;
Real fY2Min = kDiff.Dot(kBox.Axis(2)), fY2Max = fY2Min;
for (int i = 1; i < iQuantity; i++)
{
kDiff = akPoint[i] - kBox.Center();
Real fY0 = kDiff.Dot(kBox.Axis(0));
if ( fY0 < fY0Min )
fY0Min = fY0;
else if ( fY0 > fY0Max )
fY0Max = fY0;
Real fY1 = kDiff.Dot(kBox.Axis(1));
if ( fY1 < fY1Min )
fY1Min = fY1;
else if ( fY1 > fY1Max )
fY1Max = fY1;
Real fY2 = kDiff.Dot(kBox.Axis(2));
if ( fY2 < fY2Min )
fY2Min = fY2;
else if ( fY2 > fY2Max )
fY2Max = fY2;
}
kBox.Center() += (((Real)0.5)*(fY0Min+fY0Max))*kBox.Axis(0) +
(((Real)0.5)*(fY1Min+fY1Max))*kBox.Axis(1) +
(((Real)0.5)*(fY2Min+fY2Max))*kBox.Axis(2);
kBox.Extent(0) = ((Real)0.5)*(fY0Max - fY0Min);
kBox.Extent(1) = ((Real)0.5)*(fY1Max - fY1Min);
kBox.Extent(2) = ((Real)0.5)*(fY2Max - fY2Min);
return kBox;
}
void ParticleSystem::checkWallCollisions(){
float wallStiffness = 500.0;
float wallDamper = 0.5;
float radius = h*0.2;
float diff;
float adj;
Vector3 normal;
float padding = h * 0.7;
for(int i = 0; i < NumParticles; i++){
Particle * p = Particles[i];
//X Walls - Left and Right
normal = Vector3(1,0,0);
diff = 2*radius - (p->getPosition().x - (bMin.x+padding));
if(diff > EPSILON){
adj = wallStiffness * diff - wallDamper*(normal.Dot(p->getPrevVelocity()));
p->setAcceleration(p->getAcceleration() + float(adj)*normal);
}
normal = Vector3(-1,0,0);
diff = 2*radius - ((bMax.x-padding) - p->getPosition().x);
if(diff > EPSILON){
adj = wallStiffness * diff - wallDamper*(normal.Dot(p->getPrevVelocity()));
p->setAcceleration(p->getAcceleration() + float(adj)*normal);
}
//Y wall bottom
normal = Vector3(0,1,0);
diff = 2*radius - (p->getPosition().y - (bMin.y+padding));
if(diff > EPSILON){
printf("HIT\n");
adj = wallStiffness * diff - wallDamper*(normal.Dot(p->getPrevVelocity()));
p->setAcceleration(p->getAcceleration() + (float(adj)*normal));
}
//Z walls - Front and Back
normal = Vector3(0,0,1);
diff = 2*radius - (p->getPosition().z - (bMin.z+padding));
if(diff > EPSILON){
adj = wallStiffness * diff - wallDamper*(normal.Dot(p->getPrevVelocity()));
p->setAcceleration(p->getAcceleration() + float(adj)*normal);
}
normal = Vector3(0,0,-1);
diff = 2*radius - ((bMax.z-padding) - p->getPosition().z);
if(diff > EPSILON){
adj = wallStiffness * diff - wallDamper*(normal.Dot(p->getPrevVelocity()));
p->setAcceleration(p->getAcceleration() + float(adj)*normal);
}
}
}
//----------------------------------------------------------------------------
// @ BoundingSphere::Intersect()
// ---------------------------------------------------------------------------
// Determine intersection between sphere and line
//-----------------------------------------------------------------------------
bool
BoundingSphere::Intersect( const Line3& line ) const
{
// compute intermediate values
Vector3 w = mCenter - line.GetOrigin();
float wsq = w.Dot(w);
float proj = w.Dot(line.GetDirection());
float rsq = mRadius*mRadius;
float vsq = line.GetDirection().Dot(line.GetDirection());
// test length of difference vs. radius
return ( vsq*wsq - proj*proj <= vsq*rsq );
}
bool IntrLine3Box3<Real>::DoClipping ( Real t0, Real t1,
const Vector3<Real>& origin, const Vector3<Real>& direction,
const Box3<Real>& box, bool solid, int& quantity, Vector3<Real> point[2],
int& intrType )
{
// Convert linear component to box coordinates.
Vector3<Real> diff = origin - box.Center;
Vector3<Real> BOrigin(
diff.Dot( box.Axis[0] ),
diff.Dot( box.Axis[1] ),
diff.Dot( box.Axis[2] )
);
Vector3<Real> BDirection(
direction.Dot( box.Axis[0] ),
direction.Dot( box.Axis[1] ),
direction.Dot( box.Axis[2] )
);
Real saveT0 = t0, saveT1 = t1;
bool notAllClipped =
Clip( +BDirection.X(), -BOrigin.X() - box.Extent[0], t0, t1 ) &&
Clip( -BDirection.X(), +BOrigin.X() - box.Extent[0], t0, t1 ) &&
Clip( +BDirection.Y(), -BOrigin.Y() - box.Extent[1], t0, t1 ) &&
Clip( -BDirection.Y(), +BOrigin.Y() - box.Extent[1], t0, t1 ) &&
Clip( +BDirection.Z(), -BOrigin.Z() - box.Extent[2], t0, t1 ) &&
Clip( -BDirection.Z(), +BOrigin.Z() - box.Extent[2], t0, t1 );
if ( notAllClipped && ( solid || t0 != saveT0 || t1 != saveT1 ) )
{
if ( t1 > t0 )
{
intrType = IT_SEGMENT;
quantity = 2;
point[0] = origin + t0 * direction;
point[1] = origin + t1 * direction;
}
else
{
intrType = IT_POINT;
quantity = 1;
point[0] = origin + t0 * direction;
}
}
else
{
quantity = 0;
intrType = IT_EMPTY;
}
return intrType != IT_EMPTY;
}
Real DistLine3Ray3<Real>::GetSquared ()
{
Vector3<Real> kDiff = mLine->Origin - mRay->Origin;
Real a01 = -mLine->Direction.Dot(mRay->Direction);
Real b0 = kDiff.Dot(mLine->Direction);
Real c = kDiff.SquaredLength();
Real det = Math<Real>::FAbs((Real)1 - a01*a01);
Real b1, s0, s1, sqrDist;
if (det >= Math<Real>::ZERO_TOLERANCE)
{
b1 = -kDiff.Dot(mRay->Direction);
s1 = a01*b0 - b1;
if (s1 >= (Real)0)
{
// Two interior points are closest, one on line and one on ray.
Real invDet = ((Real)1)/det;
s0 = (a01*b1 - b0)*invDet;
s1 *= invDet;
sqrDist = s0*(s0 + a01*s1 + ((Real)2)*b0) +
s1*(a01*s0 + s1 + ((Real)2)*b1) + c;
}
else
{
// Origin of ray and interior point of line are closest.
s0 = -b0;
s1 = (Real)0;
sqrDist = b0*s0 + c;
}
}
else
{
// Lines are parallel, closest pair with one point at ray origin.
s0 = -b0;
s1 = (Real)0;
sqrDist = b0*s0 + c;
}
mClosestPoint0 = mLine->Origin + s0*mLine->Direction;
mClosestPoint1 = mRay->Origin + s1*mRay->Direction;
mLineParameter = s0;
mRayParameter = s1;
// Account for numerical round-off errors.
if (sqrDist < (Real)0)
{
sqrDist = (Real)0;
}
return sqrDist;
}
bool IntersectTriangle(Vector3& orig, Vector3& dir,
Vector3& v0, Vector3& v1, Vector3& v2)
{
double t, u, v;
// E1
Vector3 E1 = v1 - v0;
// E2
Vector3 E2 = v2 - v0;
// P
Vector3 P = dir.Cross(E2);
// determinant
double det = E1.Dot(P);
// keep det > 0, modify T accordingly
Vector3 T;
if( det >0 )
{
T = orig - v0;
}
else
{
T = v0 - orig;
det = -det;
}
// If determinant is near zero, ray lies in plane of triangle
if( det < 0.0001f )
return false;
// Calculate u and make sure u <= 1
u = T.Dot(P);
if( u < 0.0f || u > det )
return false;
// Q
Vector3 Q = T.Cross(E1);
// Calculate v and make sure u + v <= 1
v = dir.Dot(Q);
if( v < 0.0f || u + v > det )
return false;
// Calculate t, scale parameters, ray intersects triangle
t = E2.Dot(Q);
if (t < 0.0f) return false;
return true;
}
//----------------------------------------------------------------------------
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);
}
CylinderFit3<Real>::CylinderFit3 (int numPoints, const Vector3<Real>* points,
Vector3<Real>& center, Vector3<Real>& axis, Real& radius, Real& height,
bool inputsAreInitialGuess)
{
Real invRSqr = (Real)1;
if (!inputsAreInitialGuess)
{
// Find the least-squares line that fits the data and use it as an
// initial guess for the cylinder axis.
Line3<Real> line = OrthogonalLineFit3(numPoints, points);
center = line.Origin;
axis = line.Direction;
}
mError = Math<Real>::MAX_REAL;
const int iMax = 8;
int i;
for (i = 0; i < iMax; ++i)
{
mError = UpdateInvRSqr(numPoints, points, center, axis, invRSqr);
mError = UpdateDirection(numPoints, points, center, axis, invRSqr);
mError = UpdateCenter(numPoints, points, center, axis, invRSqr);
}
// Compute the radius.
radius = Math<Real>::InvSqrt(invRSqr);
// Project points onto fitted axis to determine extent of cylinder along
// the axis.
Real tMin = axis.Dot(points[0] - center);
Real tMax = tMin;
for (i = 1; i < numPoints; ++i)
{
Real t = axis.Dot(points[i] - center);
if (t < tMin)
{
tMin = t;
}
else if (t > tMax)
{
tMax = t;
}
}
// Compute the height. Adjust the center to point that projects to
// midpoint of extent.
height = tMax - tMin;
center += (((Real)0.5)*(tMin + tMax))*axis;
}
void Curve3<Real>::GetFrame ( Real t, Vector3<Real>& position,
Vector3<Real>& tangent, Vector3<Real>& normal, Vector3<Real>& binormal )
const
{
position = GetPosition( t );
Vector3<Real> velocity = GetFirstDerivative( t );
Vector3<Real> acceleration = GetSecondDerivative( t );
Real VDotV = velocity.Dot( velocity );
Real VDotA = velocity.Dot( acceleration );
normal = VDotV * acceleration - VDotA * velocity;
normal.Normalize();
tangent = velocity;
tangent.Normalize();
binormal = tangent.Cross( normal );
}
CylinderFit3<Real>::CylinderFit3 (int iQuantity, const Vector3<Real>* akPoint,
Vector3<Real>& rkC, Vector3<Real>& rkU, Real& rfR, Real& rfH,
bool bInputsAreInitialGuess)
{
Real fInvRSqr = (Real)1.0;
if (!bInputsAreInitialGuess)
{
// Find the least-squares line that fits the data and use it as an
// initial guess for the cylinder axis.
Line3<Real> kLine = OrthogonalLineFit3(iQuantity,akPoint);
rkC = kLine.Origin;
rkU = kLine.Direction;
}
m_fError = Math<Real>::MAX_REAL;
const int iMax = 8;
int i;
for (i = 0; i < iMax; i++)
{
m_fError = UpdateInvRSqr(iQuantity,akPoint,rkC,rkU,fInvRSqr);
m_fError = UpdateDirection(iQuantity,akPoint,rkC,rkU,fInvRSqr);
m_fError = UpdateCenter(iQuantity,akPoint,rkC,rkU,fInvRSqr);
}
// Compute the radius.
rfR = Math<Real>::InvSqrt(fInvRSqr);
// Project points onto fitted axis to determine extent of cylinder along
// the axis.
Real fTMin = rkU.Dot(akPoint[0]-rkC), fTMax = fTMin;
for (i = 1; i < iQuantity; i++)
{
Real fT = rkU.Dot(akPoint[i]-rkC);
if (fT < fTMin)
{
fTMin = fT;
}
else if (fT > fTMax)
{
fTMax = fT;
}
}
// Compute the height. Adjust the center to point that projects to
// midpoint of extent.
rfH = fTMax - fTMin;
rkC += ((Real)0.5)*(fTMin+fTMax)*rkU;
}
void ObjectRenderer::RenderShadow(int pass, const Vector3& light, const Vector3& observer){
if(pass<0) return;
glPushMatrix();
Vector3 objLight;
Vector3 objObserver;
Vector3 obsObj = mObject->GetReferenceFrame().GetOrigin();
obsObj -= observer;
Vector3 ligObj = mObject->GetReferenceFrame().GetOrigin();
ligObj -= light;
glMultMatrixf(mObject->GetReferenceFrame().GetHMatrix().RowOrderForceFloat());
mObject->GetReferenceFrame().GetInverse().GetHMatrix().Transform(light, objLight);
mObject->GetReferenceFrame().GetInverse().GetHMatrix().Transform(observer, objObserver);
for(int i=0;i<int(mShapes.size());i++){
if(mShapes[i]->shape){
GL3DObject::RenderShadowInitPass(pass +(obsObj.Dot(ligObj)>0?0:2));
mShapes[i]->shape->RenderShadowPass(pass, objLight);
}
}
glPopMatrix();
AbstractRenderer::RenderShadow(pass,light,observer);
}
Real DistVector3Box3<Real>::GetSquared ()
{
// work in the box's coordinate system
Vector3<Real> kDiff = *m_pkVector - m_pkBox->Center;
// compute squared distance and closest point on box
Real fSqrDistance = (Real)0.0, fDelta;
Vector3<Real> kClosest;
int i;
for (i = 0; i < 3; i++)
{
kClosest[i] = kDiff.Dot(m_pkBox->Axis[i]);
if (kClosest[i] < -m_pkBox->Extent[i])
{
fDelta = kClosest[i] + m_pkBox->Extent[i];
fSqrDistance += fDelta*fDelta;
kClosest[i] = -m_pkBox->Extent[i];
}
else if (kClosest[i] > m_pkBox->Extent[i])
{
fDelta = kClosest[i] - m_pkBox->Extent[i];
fSqrDistance += fDelta*fDelta;
kClosest[i] = m_pkBox->Extent[i];
}
}
m_kClosestPoint0 = *m_pkVector;
m_kClosestPoint1 = m_pkBox->Center;
for (i = 0; i < 3; i++)
{
m_kClosestPoint1 += kClosest[i]*m_pkBox->Axis[i];
}
return fSqrDistance;
}
bool IntrSphere3Sphere3<Real>::Test ( Real tmax,
const Vector3<Real>& velocity0, const Vector3<Real>& velocity1 )
{
Vector3<Real> relVelocity = velocity1 - velocity0;
Real a = relVelocity.SquaredLength();
Vector3<Real> CDiff = mSphere1->Center - mSphere0->Center;
Real c = CDiff.SquaredLength();
Real rSum = mSphere0->Radius + mSphere1->Radius;
Real rSumSqr = rSum * rSum;
if ( a > ( Real )0 )
{
Real b = CDiff.Dot( relVelocity );
if ( b <= ( Real )0 )
{
if ( -tmax * a <= b )
{
return a * c - b * b <= a * rSumSqr;
}
else
{
return tmax * ( tmax * a + ( ( Real )2 ) * b ) + c <= rSumSqr;
}
}
}
return c <= rSumSqr;
}
bool Wml::Culled (const Plane3<Real>& rkPlane,
const Ellipsoid3<Real>& rkEllipsoid, bool bUnitNormal)
{
Vector3<Real> kNormal = rkPlane.GetNormal();
Real fConstant = rkPlane.GetConstant();
if ( !bUnitNormal )
{
Real fLength = kNormal.Normalize();
fConstant /= fLength;
}
Real fDiscr = kNormal.Dot(rkEllipsoid.InverseA()*kNormal);
Real fRoot = Math<Real>::Sqrt(Math<Real>::FAbs(fDiscr));
Real fSDist = kNormal.Dot(rkEllipsoid.Center()) - fConstant;
return fSDist <= -fRoot;
}
//----------------------------------------------------------------------------
bool Mgc::TestIntersection (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 Math::FAbs(fSDist) <= fRoot;
}
//----------------------------------------------------------------------------
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 FrustumPickVisitor::AddHitTriangle(const Vector3& v0,const Vector3& v1,const Vector3& v2)
{
float32_t dot = 0.f;
Vector3 normal ( (v2 - v1).Cross (v1 - v0) );
if (m_Camera->GetShadingMode() != ShadingMode::Wireframe && m_Camera->IsBackFaceCulling())
{
Vector3 cameraDir;
m_Camera->GetDirection (cameraDir);
m_CurrentInverseWorldTransform.TransformNormal (cameraDir);
dot = normal.Dot (cameraDir);
}
if ( dot >= 0.f )
{
// allocate a hit
PickHit* hit = AddHit();
// our intersection point is center of the triangle (HACK)
Vector3 intersection ( ( v0 + v1 + v2 ) / 3.f );
// transform values into world space
m_CurrentWorldTransform.TransformVertex(intersection);
// set intersection in world space (we use FLT_MAX for the distance since the distance is spacial)
if (!HasFlags(PickFlags::IgnoreIntersection))
{
hit->SetIntersection( intersection );
}
return true;
}
return false;
}
bool IntrSphere3Sphere3<Real>::Test (Real fTMax,
const Vector3<Real>& rkVelocity0, const Vector3<Real>& rkVelocity1)
{
Vector3<Real> kVDiff = rkVelocity1 - rkVelocity0;
Real fA = kVDiff.SquaredLength();
Vector3<Real> kCDiff = m_rkSphere1.Center - m_rkSphere0.Center;
Real fC = kCDiff.SquaredLength();
Real fRSum = m_rkSphere0.Radius + m_rkSphere1.Radius;
Real fRSumSqr = fRSum*fRSum;
if (fA > (Real)0.0)
{
Real fB = kCDiff.Dot(kVDiff);
if (fB <= (Real)0.0)
{
if (-fTMax*fA <= fB)
{
return fA*fC - fB*fB <= fA*fRSumSqr;
}
else
{
return fTMax*(fTMax*fA + ((Real)2.0)*fB) + fC <= fRSumSqr;
}
}
}
return fC <= fRSumSqr;
}
Real Mgc::SqrDistance (const Vector3& rkPoint, const Line3& rkLine,
Real* pfParam)
{
Vector3 kDiff = rkPoint - rkLine.Origin();
Real fSqrLen = rkLine.Direction().SquaredLength();
Real fT = kDiff.Dot(rkLine.Direction())/fSqrLen;
kDiff -= fT*rkLine.Direction();
if ( pfParam )
*pfParam = fT;
return kDiff.SquaredLength();
}
//----------------------------------------------------------------------------
// @ ::Merge()
// ---------------------------------------------------------------------------
// Merge two spheres together to create a new one
//-----------------------------------------------------------------------------
void
Merge( BoundingSphere& result,
const BoundingSphere& s0, const BoundingSphere& s1 )
{
// get differences between them
Vector3 diff = s1.mCenter - s0.mCenter;
float distsq = diff.Dot(diff);
float radiusdiff = s1.mRadius - s0.mRadius;
// if one sphere inside other
if ( distsq <= radiusdiff*radiusdiff )
{
if ( s0.mRadius > s1.mRadius )
result = s0;
else
result = s1;
return;
}
// build new sphere
float dist = Sqrt( distsq );
float radius = 0.5f*( s0.mRadius + s1.mRadius + dist );
Vector3 center = s0.mCenter;
if (!gfx::IsZero( dist ))
center += ((radius-s0.mRadius)/dist)*diff;
result.SetRadius( radius );
result.SetCenter( center );
} // End of ::Merge()
void Triangle::computeForces(Vector3 &airVelocity)
{
this->airVelocity = airVelocity;
Vector3 velocity = (p1->velocity + p2->velocity + p3->velocity) / 3;
Vector3 realVelocity = velocity - airVelocity;
if (realVelocity.Mag() > 0)
{
Vector3 *one = new Vector3();
Vector3 *two = new Vector3();
one->Cross(p2->position - p1->position, p3->position - p1->position);
two->Cross(p2->position - p1->position, p3->position - p1->position);
one->Normalize();
normal = *one;
float a0 = 0.5 * two->Mag();
float a = a0 * (realVelocity.Dot(normal) / realVelocity.Mag());
Vector3 fAero = -0.5 * density * realVelocity.Mag2() *a *normal;
Vector3 fAeroParticle = fAero / 3;
if (!p1->pinned)
p1->applyForce(fAeroParticle);
if (!p2->pinned)
p2->applyForce(fAeroParticle);
if (!p3->pinned)
p3->applyForce(fAeroParticle);
delete one;
delete two;
}
}
bool IntrSegment3Triangle3<Real>::Test ()
{
// Compute the offset origin, edges, and normal.
Vector3<Real> diff = mSegment->Center - mTriangle->V[0];
Vector3<Real> edge1 = mTriangle->V[1] - mTriangle->V[0];
Vector3<Real> edge2 = mTriangle->V[2] - mTriangle->V[0];
Vector3<Real> normal = edge1.Cross(edge2);
// Solve Q + t*D = b1*E1 + b2*E2 (Q = diff, D = segment direction,
// E1 = edge1, E2 = edge2, N = Cross(E1,E2)) by
// |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2))
// |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q))
// |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N)
Real DdN = mSegment->Direction.Dot(normal);
Real sign;
if (DdN > Math<Real>::ZERO_TOLERANCE)
{
sign = (Real)1;
}
else if (DdN < -Math<Real>::ZERO_TOLERANCE)
{
sign = (Real)-1;
DdN = -DdN;
}
else
{
// Segment and triangle are parallel, call it a "no intersection"
// even if the segment does intersect.
mIntersectionType = IT_EMPTY;
return false;
}
Real DdQxE2 = sign*mSegment->Direction.Dot(diff.Cross(edge2));
if (DdQxE2 >= (Real)0)
{
Real DdE1xQ = sign*mSegment->Direction.Dot(edge1.Cross(diff));
if (DdE1xQ >= (Real)0)
{
if (DdQxE2 + DdE1xQ <= DdN)
{
// Line intersects triangle, check if segment does.
Real QdN = -sign*diff.Dot(normal);
Real extDdN = mSegment->Extent*DdN;
if (-extDdN <= QdN && QdN <= extDdN)
{
// Segment intersects triangle.
mIntersectionType = IT_POINT;
return true;
}
// else: |t| > extent, no intersection
}
// else: b1+b2 > 1, no intersection
}
// else: b2 < 0, no intersection
}
// else: b1 < 0, no intersection
mIntersectionType = IT_EMPTY;
return false;
}
void pTriangle::ComputeForce(Vector3 vair) {
//find the tirangle velocity
velocity = (p1->getVelocity() + p2->getVelocity() + p3->getVelocity()) / 3 ;
//assume no air velocity for now
Vector3 v = velocity - vair;
Vector3 r1 = p1->getPosition();
Vector3 r2 = p2->getPosition();
Vector3 r3 = p3->getPosition();
//find triangle unnormalized normal, n*
Vector3 ra = (r2 - r1);
Vector3 rb = (r3 - r1);
Vector3 ncross;
ncross.Cross(ra, rb);
//constants
float p = 1.225f;
float cd = 1.225f;
//force = -1/2 * p * |v|^2 * cd * a * n
Vector3 v2an = ( (v.Mag()*v.Dot(ncross))/(2.0f*ncross.Mag()) ) * ncross;
Vector3 aeroForce = (-1.0f/2.0f) * p * cd * v2an;
aeroForce = aeroForce / 3;
p1->ApplyForce(aeroForce);
p2->ApplyForce(aeroForce);
p3->ApplyForce(aeroForce);
normal = ncross.Normalize();
}