bool IntrSegment2Box2<Real>::Test ()
{
Vector2<Real> kDiff = m_rkSegment.Origin - m_rkBox.Center;
Real fAWdU[2], fADdU[2], fRhs;
fAWdU[0] = Math<Real>::FAbs(m_rkSegment.Direction.Dot(m_rkBox.Axis[0]));
fADdU[0] = Math<Real>::FAbs(kDiff.Dot(m_rkBox.Axis[0]));
fRhs = m_rkBox.Extent[0] + m_rkSegment.Extent*fAWdU[0];
if (fADdU[0] > fRhs)
{
return false;
}
fAWdU[1] = Math<Real>::FAbs(m_rkSegment.Direction.Dot(m_rkBox.Axis[1]));
fADdU[1] = Math<Real>::FAbs(kDiff.Dot(m_rkBox.Axis[1]));
fRhs = m_rkBox.Extent[1] + m_rkSegment.Extent*fAWdU[1];
if (fADdU[1] > fRhs)
{
return false;
}
Vector2<Real> kPerp = m_rkSegment.Direction.Perp();
Real fLhs = Math<Real>::FAbs(kPerp.Dot(kDiff));
Real fPart0 = Math<Real>::FAbs(kPerp.Dot(m_rkBox.Axis[0]));
Real fPart1 = Math<Real>::FAbs(kPerp.Dot(m_rkBox.Axis[1]));
fRhs = m_rkBox.Extent[0]*fPart0 + m_rkBox.Extent[1]*fPart1;
return fLhs <= fRhs;
}
bool IntrRay2Box2<Real>::Test ()
{
Real WdU[2], AWdU[2], DdU[2], ADdU[2];
Vector2<Real> diff = mRay->Origin - mBox->Center;
WdU[0] = mRay->Direction.Dot(mBox->Axis[0]);
AWdU[0] = Math<Real>::FAbs(WdU[0]);
DdU[0] = diff.Dot(mBox->Axis[0]);
ADdU[0] = Math<Real>::FAbs(DdU[0]);
if (ADdU[0] > mBox->Extent[0] && DdU[0]*WdU[0] >= (Real)0)
{
return false;
}
WdU[1] = mRay->Direction.Dot(mBox->Axis[1]);
AWdU[1] = Math<Real>::FAbs(WdU[1]);
DdU[1] = diff.Dot(mBox->Axis[1]);
ADdU[1] = Math<Real>::FAbs(DdU[1]);
if (ADdU[1] > mBox->Extent[1] && DdU[1]*WdU[1] >= (Real)0)
{
return false;
}
Vector2<Real> perp = mRay->Direction.Perp();
Real LHS = Math<Real>::FAbs(perp.Dot(diff));
Real part0 = Math<Real>::FAbs(perp.Dot(mBox->Axis[0]));
Real part1 = Math<Real>::FAbs(perp.Dot(mBox->Axis[1]));
Real RHS = mBox->Extent[0]*part0 + mBox->Extent[1]*part1;
return LHS <= RHS;
}
bool IntrSegment2Box2<Real>::Test ()
{
Vector2<Real> diff = mSegment->Center - mBox->Center;
Real AWdU[2], ADdU[2], RHS;
AWdU[0] = Math<Real>::FAbs(mSegment->Direction.Dot(mBox->Axis[0]));
ADdU[0] = Math<Real>::FAbs(diff.Dot(mBox->Axis[0]));
RHS = mBox->Extent[0] + mSegment->Extent*AWdU[0];
if (ADdU[0] > RHS)
{
return false;
}
AWdU[1] = Math<Real>::FAbs(mSegment->Direction.Dot(mBox->Axis[1]));
ADdU[1] = Math<Real>::FAbs(diff.Dot(mBox->Axis[1]));
RHS = mBox->Extent[1] + mSegment->Extent*AWdU[1];
if (ADdU[1] > RHS)
{
return false;
}
Vector2<Real> perp = mSegment->Direction.Perp();
Real LHS = Math<Real>::FAbs(perp.Dot(diff));
Real part0 = Math<Real>::FAbs(perp.Dot(mBox->Axis[0]));
Real part1 = Math<Real>::FAbs(perp.Dot(mBox->Axis[1]));
RHS = mBox->Extent[0]*part0 + mBox->Extent[1]*part1;
return LHS <= RHS;
}
//----------------------------------------------------------------------------
bool Mgc::ContOrientedBox (int iQuantity, const Vector2* akPoint,
const bool* abValid, Box2& rkBox)
{
if ( !GaussPointsFit(iQuantity,akPoint,abValid,rkBox.Center(),
rkBox.Axes(),rkBox.Extents()) )
{
return false;
}
// Let C be the box center and let U0 and U1 be the box axes. Each input
// point is of the form X = C + y0*U0 + y1*U1. The following code
// computes min(y0), max(y0), min(y1), and max(y1). The box center is
// then adjusted to be
// C' = C + 0.5*(min(y0)+max(y0))*U0 + 0.5*(min(y1)+max(y1))*U1
// get first valid vertex
Vector2 kDiff;
Real fY0Min, fY0Max, fY1Min, fY1Max;
int i;
for (i = 0; i < iQuantity; i++)
{
if ( abValid[i] )
{
kDiff = akPoint[i] - rkBox.Center();
fY0Min = kDiff.Dot(rkBox.Axis(0));
fY0Max = fY0Min;
fY1Min = kDiff.Dot(rkBox.Axis(1));
fY1Max = fY1Min;
break;
}
}
for (i++; i < iQuantity; i++)
{
if ( abValid[i] )
{
kDiff = akPoint[i] - rkBox.Center();
Real fY0 = kDiff.Dot(rkBox.Axis(0));
if ( fY0 < fY0Min )
fY0Min = fY0;
else if ( fY0 > fY0Max )
fY0Max = fY0;
Real fY1 = kDiff.Dot(rkBox.Axis(1));
if ( fY1 < fY1Min )
fY1Min = fY1;
else if ( fY1 > fY1Max )
fY1Max = fY1;
}
}
rkBox.Center() += (0.5f*(fY0Min+fY0Max))*rkBox.Axis(0)
+ (0.5f*(fY1Min+fY1Max))*rkBox.Axis(1);
rkBox.Extent(0) = 0.5f*(fY0Max - fY0Min);
rkBox.Extent(1) = 0.5f*(fY1Max - fY1Min);
return true;
}
void CurrencyOrb::DoTrackPlayer(float delta)
{
mSineWaveProps.DoSineWave = false;
Player * player = GameObjectManager::Instance()->GetPlayer();
if (!player)
{
return;
}
// accelerate towards the target
Vector3 direction = Vector3(player->CollisionCentreX(), player->CollisionCentreY(), player->Z()) - m_position;
direction.Normalise();
m_direction = direction;
float multiplier = mTimeTracking > 2.0f ? 3.0f : 1.0f;
bool prioritiseX = false;
if (std::abs(m_direction.X) > std::abs(m_direction.Y))
{
AccelerateX(m_direction.X, kAccelerateRate * multiplier);
prioritiseX = true;
}
else
{
AccelerateY(m_direction.Y, kAccelerateRate * multiplier);
}
if (prioritiseX && ((m_direction.X < 0 && m_velocity.X > 0) ||
(m_direction.X > 0 && m_velocity.X < 0)))
{
// the velocity of the orb is still moving in the opposite x direction
// let's give it a helping hand to catch up by accelerating harshly
AccelerateX(m_direction.X, kHarshAccelerateRate * multiplier);
}
if (!prioritiseX && ((m_direction.Y < 0 && m_velocity.Y > 0) ||
(m_direction.Y > 0 && m_velocity.Y < 0)))
{
// the velocity of the orb is still moving in the opposite y direction
// let's give it a helping hand to catch up by accelerating harshly
AccelerateY(m_direction.Y, kHarshAccelerateRate * multiplier);
}
Vector2 dir = Vector2(m_velocity.X, m_velocity.Y);
dir.Normalise();
if (dir.X > 0)
{
SetRotationAngle(-acos(dir.Dot(Vector2(0, -1))));
}
else
{
SetRotationAngle(acos(dir.Dot(Vector2(0, -1))));
}
}
bool IntrLine2Box2<Real>::Test ()
{
Vector2<Real> diff = mLine->Origin - mBox->Center;
Vector2<Real> perp = mLine->Direction.Perp();
Real LHS = Math<Real>::FAbs( perp.Dot( diff ) );
Real part0 = Math<Real>::FAbs( perp.Dot( mBox->Axis[0] ) );
Real part1 = Math<Real>::FAbs( perp.Dot( mBox->Axis[1] ) );
Real RHS = mBox->Extent[0] * part0 + mBox->Extent[1] * part1;
return LHS <= RHS;
}
Real DistLine2Ray2<Real>::GetSquared ()
{
Vector2<Real> diff = mLine->Origin - mRay->Origin;
Real a01 = -mLine->Direction.Dot(mRay->Direction);
Real b0 = diff.Dot(mLine->Direction);
Real c = diff.SquaredLength();
Real det = Math<Real>::FAbs((Real)1 - a01*a01);
Real b1, s0, s1, sqrDist;
if (det >= Math<Real>::ZERO_TOLERANCE)
{
b1 = -diff.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 = (Real)0;
}
else
{
// Origin of ray and interior point of line are closest.
s0 = -b0;
s1 = (Real)0;
sqrDist = b0*s0 + c;
// Account for numerical round-off errors.
if (sqrDist < (Real)0)
{
sqrDist = (Real)0;
}
}
}
else
{
// Lines are parallel, closest pair with one point at ray origin.
s0 = -b0;
s1 = (Real)0;
sqrDist = b0*s0 + c;
// Account for numerical round-off errors.
if (sqrDist < (Real)0)
{
sqrDist = (Real)0;
}
}
mClosestPoint0 = mLine->Origin + s0*mLine->Direction;
mClosestPoint1 = mRay->Origin + s1*mRay->Direction;
return sqrDist;
}
bool IntrTriangle2Triangle2<Real>::Test (Real tmax,
const Vector2<Real>& velocity0, const Vector2<Real>& velocity1)
{
// Process as if V0-triangle is stationary and V1-triangle is moving.
Vector2<Real> W = velocity1 - velocity0;
int side = 0; // 0 = NONE, -1 = LEFT, +1 = RIGHT
Real tfirst = (Real)0;
Real tlast = Math<Real>::MAX_REAL;
Configuration cfg0, cfg1, tcfg0, tcfg1;
int i0, i1, i2;
Vector2<Real> D;
Real speed;
// Process edges of V0-triangle.
for (i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2++)
{
// Test axis V0[i1] + t*perp(V0[i2]-V0[i1]), perp(x,y) = (y,-x).
D.X() = mTriangle0->V[i2].Y() - mTriangle0->V[i1].Y();
D.Y() = mTriangle0->V[i1].X() - mTriangle0->V[i2].X();
speed = D.Dot(W);
ComputeTwo(cfg0, mTriangle0->V, D, i0, i1, i2);
ComputeThree(cfg1, mTriangle1->V, D, mTriangle0->V[i1]);
if (NoIntersect(cfg0, cfg1, tmax, speed, side, tcfg0, tcfg1,
tfirst, tlast))
{
return false;
}
}
// Process edges of V1-triangle.
for (i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2++)
{
// Test axis V1[i1] + t*perp(V1[i2]-V1[i1]), perp(x,y) = (y,-x).
D.X() = mTriangle1->V[i2].Y() - mTriangle1->V[i1].Y();
D.Y() = mTriangle1->V[i1].X() - mTriangle1->V[i2].X();
speed = D.Dot(W);
ComputeTwo(cfg1, mTriangle1->V, D, i0, i1, i2);
ComputeThree(cfg0, mTriangle0->V, D, mTriangle1->V[i1]);
if (NoIntersect(cfg0, cfg1, tmax, speed, side, tcfg0, tcfg1,
tfirst, tlast))
{
return false;
}
}
mContactTime = tfirst;
return true;
}
float Circle::RayCast( Vector2 *start, Vector2 *end, bool invert ){
Vector2 *delta = start->Sub(m_pos);
Vector2 *d = end->Sub(start);
float a = d->Dot(delta);
float b = d->get_m_LenSqr();
float c = delta->get_m_LenSqr();
float roa = a * a - b * (c - m_radius * m_radius);
if (roa < 0){
// no intersection
return -1;
} else if (roa == 0) {
// ray tangent to circle, one intersection
float num = sqrt(roa);
float t0 = (-a + num) / b;
return t0;
} else {
// two intersection points
float num = sqrt(roa);
float t0 = (-a + num) / b;
float t1 = (-a - num) / b;
if (invert) {
return (float)_min(t1, t0);
} else {
return t0;
}
}
}
bool IntrLine2Circle2<Real>::Find (const Vector2<Real>& origin,
const Vector2<Real>& direction, const Vector2<Real>& center,
Real radius, int& rootCount, Real t[2])
{
// Intersection of a the line P+t*D and the circle |X-C| = R. The line
// direction is unit length. The t value is a root to the quadratic
// equation:
// 0 = |t*D+P-C|^2 - R^2
// = t^2 + 2*Dot(D,P-C)*t + |P-C|^2-R^2
// = t^2 + 2*a1*t + a0
// If two roots are returned, the order is T[0] < T[1].
Vector2<Real> diff = origin - center;
Real a0 = diff.SquaredLength() - radius*radius;
Real a1 = direction.Dot(diff);
Real discr = a1*a1 - a0;
if (discr > Math<Real>::ZERO_TOLERANCE)
{
rootCount = 2;
discr = Math<Real>::Sqrt(discr);
t[0] = -a1 - discr;
t[1] = -a1 + discr;
}
else if (discr < -Math<Real>::ZERO_TOLERANCE)
{
rootCount = 0;
}
else // discr == 0
{
rootCount = 1;
t[0] = -a1;
}
return rootCount != 0;
}
Real DistPoint2Ellipse2<Real>::GetSquared ()
{
// Compute coordinates of point in ellipse coordinate system.
Vector2<Real> diff = *mPoint - mEllipse->Center;
Vector2<Real> y(diff.Dot(mEllipse->Axis[0]), diff.Dot(mEllipse->Axis[1]));
Vector2<Real> x;
Real sqrDistance = SqrDistance(mEllipse->Extent, y, x);
mClosestPoint0 = *mPoint;
mClosestPoint1 = mEllipse->Center +
x[0]*mEllipse->Axis[0] +
x[1]*mEllipse->Axis[1];
return sqrDistance;
}
bool
collides( const Circle & c, const LineSegment & l)
{
Vector2 posToCenter = c.GetCenter() - l.GetStart();
Vector2 dirVec = l.GetEnd() - l.GetStart();
float segmentLength = dirVec.Length();
// direction vector must be unit vector (ie. length = 1) in this case!
// otherwise we would need another formula for scalar projection.
dirVec = dirVec / segmentLength;
// scalar projection of posToCenter to direction vector.
float d = dirVec.Dot(posToCenter);
// if d value exceeds original segment length, then we put a cap on it.
// if these two lines are dismissed, then algorithm sees line segment
// as a infinite line.
if ( d > segmentLength ) d = segmentLength;
if ( d < -segmentLength ) d = -segmentLength;
// compute closest point to circle center from line start
// along direction vector.
Vector2 closest_point =l.GetStart() + dirVec * d;
// vectorfrom circle center to closest point on line
Vector2 S = closest_point - c.GetCenter();
return (S.Length() <= c.GetRadius());
}
float Vector2::AngleBetween(Vector2 v) const {
if (this->Magnitude() == 0 || v.Magnitude() == 0) {
return 0;
}
Vector2 left = this->Normalize();
Vector2 right = v.Normalize();
if (left == right) {
return 0;
}
float dot = left.Dot(right);
// Floating points check
if (dot > 1.0f) {
dot = 1.0f;
}
else if (dot < -1.0f) {
dot = -1.0f;
}
float rot = acos(dot);
// http://stackoverflow.com/questions/11022446/direction-of-shortest-rotation-between-two-vectors
// Use cross vector3 to determine direction
Vector3 cross = Vector3(left.x, left.y).Cross(Vector3(right.x, right.y));
if (cross.z > 0) {
return -rot;
}
else {
return rot;
}
}
bool IntrLine2Circle2<Real>::Find (const Vector2<Real>& rkOrigin,
const Vector2<Real>& rkDirection, const Vector2<Real>& rkCenter,
Real fRadius, int& riRootCount, Real afT[2])
{
// Intersection of a the line P+t*D and the circle |X-C| = R. The line
// direction is unit length. The t value is a root to the quadratic
// equation:
// 0 = |t*D+P-C|^2 - R^2
// = t^2 + 2*Dot(D,P-C)*t + |P-C|^2-R^2
// = t^2 + 2*a1*t + a0
// If two roots are returned, the order is T[0] < T[1].
Vector2<Real> kDiff = rkOrigin - rkCenter;
Real fA0 = kDiff.SquaredLength() - fRadius*fRadius;
Real fA1 = rkDirection.Dot(kDiff);
Real fDiscr = fA1*fA1 - fA0;
if (fDiscr > Math<Real>::ZERO_TOLERANCE)
{
riRootCount = 2;
fDiscr = Math<Real>::Sqrt(fDiscr);
afT[0] = -fA1 - fDiscr;
afT[1] = -fA1 + fDiscr;
}
else if (fDiscr < -Math<Real>::ZERO_TOLERANCE)
{
riRootCount = 0;
}
else // fDiscr == 0
{
riRootCount = 1;
afT[0] = -fA1;
}
return riRootCount != 0;
}
int IntrTriangle2Triangle2<Real>::WhichSide (const Vector2<Real> V[3],
const Vector2<Real>& P, const Vector2<Real>& D)
{
// Vertices are projected to the form P+t*D. Return value is +1 if all
// t > 0, -1 if all t < 0, 0 otherwise, in which case the line splits the
// triangle.
int positive = 0, negative = 0, zero = 0;
for (int i = 0; i < 3; ++i)
{
Real t = D.Dot(V[i] - P);
if (t > (Real)0)
{
++positive;
}
else if (t < (Real)0)
{
++negative;
}
else
{
++zero;
}
if (positive > 0 && negative > 0)
{
return 0;
}
}
return (zero == 0 ? (positive > 0 ? 1 : -1) : 0);
}
bool IntrLine2Box2<Real>::DoClipping ( Real t0, Real t1,
const Vector2<Real>& origin, const Vector2<Real>& direction,
const Box2<Real>& box, bool solid, int& quantity, Vector2<Real> point[2],
int& intrType )
{
// Convert linear component to box coordinates.
Vector2<Real> diff = origin - box.Center;
Vector2<Real> BOrigin(
diff.Dot( box.Axis[0] ),
diff.Dot( box.Axis[1] )
);
Vector2<Real> BDirection(
direction.Dot( box.Axis[0] ),
direction.Dot( box.Axis[1] )
);
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 );
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
{
intrType = IT_EMPTY;
quantity = 0;
}
return intrType != IT_EMPTY;
}
int SeparatePoints2<Real>::OnSameSide (const Vector2<Real>& lineNormal,
Real lineConstant, int numEdges, const int* edges,
const Vector2<Real>* points)
{
// Test whether all points on same side of line Dot(N,X) = c.
Real c0;
int posSide = 0, negSide = 0;
for (int i1 = 0, i0 = numEdges-1; i1 < numEdges; i0 = i1++)
{
c0 = lineNormal.Dot(points[edges[i0]]);
if (c0 > lineConstant + Math<Real>::ZERO_TOLERANCE)
{
++posSide;
}
else if (c0 < lineConstant - Math<Real>::ZERO_TOLERANCE)
{
++negSide;
}
if (posSide && negSide)
{
// Line splits point set.
return 0;
}
c0 = lineNormal.Dot(points[edges[i1]]);
if (c0 > lineConstant + Math<Real>::ZERO_TOLERANCE)
{
++posSide;
}
else if (c0 < lineConstant - Math<Real>::ZERO_TOLERANCE)
{
++negSide;
}
if (posSide && negSide)
{
// Line splits point set.
return 0;
}
}
return (posSide ? +1 : -1);
}
void IntrLine2Triangle2<Real>::GetInterval (const Vector2<Real>& rkOrigin,
const Vector2<Real>& rkDirection, const Triangle2<Real>& rkTriangle,
const Real afDist[3], const int aiSign[3], Real afParam[2])
{
// Project triangle onto line.
Real afProj[3];
int i;
for (i = 0; i < 3; i++)
{
Vector2<Real> kDiff = rkTriangle.V[i] - rkOrigin;
afProj[i] = rkDirection.Dot(kDiff);
}
// Compute transverse intersections of triangle edges with line.
Real fNumer, fDenom;
int i0, i1, i2;
int iQuantity = 0;
for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
if (aiSign[i0]*aiSign[i1] < 0)
{
assert(iQuantity < 2);
fNumer = afDist[i0]*afProj[i1] - afDist[i1]*afProj[i0];
fDenom = afDist[i0] - afDist[i1];
afParam[iQuantity++] = fNumer/fDenom;
}
}
// Check for grazing contact.
if (iQuantity < 2)
{
for (i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2++)
{
if (aiSign[i2] == 0)
{
assert(iQuantity < 2);
afParam[iQuantity++] = afProj[i2];
}
}
}
// Sort.
assert(iQuantity >= 1);
if (iQuantity == 2)
{
if (afParam[0] > afParam[1])
{
Real fSave = afParam[0];
afParam[0] = afParam[1];
afParam[1] = fSave;
}
}
else
{
afParam[1] = afParam[0];
}
}
void IntrLine2Triangle2<Real>::GetInterval (const Vector2<Real>& origin,
const Vector2<Real>& direction, const Triangle2<Real>& triangle,
const Real dist[3], const int sign[3], Real param[2])
{
// Project triangle onto line.
Real proj[3];
int i;
for (i = 0; i < 3; ++i)
{
Vector2<Real> diff = triangle.V[i] - origin;
proj[i] = direction.Dot(diff);
}
// Compute transverse intersections of triangle edges with line.
Real numer, denom;
int i0, i1, i2;
int quantity = 0;
for (i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
if (sign[i0]*sign[i1] < 0)
{
assertion(quantity < 2, "Too many intersections\n");
numer = dist[i0]*proj[i1] - dist[i1]*proj[i0];
denom = dist[i0] - dist[i1];
param[quantity++] = numer/denom;
}
}
// Check for grazing contact.
if (quantity < 2)
{
for (i0 = 1, i1 = 2, i2 = 0; i2 < 3; i0 = i1, i1 = i2++)
{
if (sign[i2] == 0)
{
assertion(quantity < 2, "Too many intersections\n");
param[quantity++] = proj[i2];
}
}
}
// Sort.
assertion(quantity >= 1, "Need at least one intersection\n");
if (quantity == 2)
{
if (param[0] > param[1])
{
Real save = param[0];
param[0] = param[1];
param[1] = save;
}
}
else
{
param[1] = param[0];
}
}
//----------------------------------------------------------------------------
Box2 Mgc::ContOrientedBox (int iQuantity, const Vector2* akPoint)
{
Box2 kBox;
GaussPointsFit(iQuantity,akPoint,kBox.Center(),kBox.Axes(),
kBox.Extents());
// Let C be the box center and let U0 and U1 be the box axes. Each input
// point is of the form X = C + y0*U0 + y1*U1. The following code
// computes min(y0), max(y0), min(y1), and max(y1). The box center is
// then adjusted to be
// C' = C + 0.5*(min(y0)+max(y0))*U0 + 0.5*(min(y1)+max(y1))*U1
Vector2 kDiff = akPoint[0] - kBox.Center();
Real fY0Min = kDiff.Dot(kBox.Axis(0)), fY0Max = fY0Min;
Real fY1Min = kDiff.Dot(kBox.Axis(1)), fY1Max = fY1Min;
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;
}
kBox.Center() += (0.5f*(fY0Min+fY0Max))*kBox.Axis(0) +
(0.5f*(fY1Min+fY1Max))*kBox.Axis(1);
kBox.Extent(0) = 0.5f*(fY0Max - fY0Min);
kBox.Extent(1) = 0.5f*(fY1Max - fY1Min);
return kBox;
}
bool IntrTriangle3Cylinder3<Real>::DiskOverlapsSegment (
const Vector2<Real>& Q0, const Vector2<Real>& Q1 ) const
{
Real rSqr = mCylinder->Radius * mCylinder->Radius;
Vector2<Real> D = Q0 - Q1;
Real dot = Q0.Dot( D );
if ( dot <= ( Real )0 )
{
return Q0.Dot( Q0 ) <= rSqr;
}
Real lenSqr = D.Dot( D );
if ( dot >= lenSqr )
{
return Q1.Dot( Q1 ) <= rSqr;
}
dot = D.DotPerp( Q0 );
return dot * dot <= lenSqr * rSqr;
}
void IntrTriangle2Triangle2<Real>::ComputeTwo (Configuration& cfg,
const Vector2<Real> V[3], const Vector2<Real>& D, int i0, int i1,
int i2)
{
cfg.Map = M12;
cfg.Index[0] = i0;
cfg.Index[1] = i1;
cfg.Index[2] = i2;
cfg.Min = D.Dot(V[i0] - V[i1]);
cfg.Max = (Real)0;
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Ray2& rkRay0, const Ray2& rkRay1,
int& riQuantity, Real afT[2])
{
Vector2 kDiff;
Real fD0SqrLen;
bool bIntersects = Find(rkRay0.Origin(),rkRay0.Direction(),
rkRay1.Origin(),rkRay1.Direction(),kDiff,fD0SqrLen,riQuantity,afT);
if ( bIntersects )
{
if ( riQuantity == 1 )
{
if ( afT[0] < 0.0f || afT[1] < 0.0f )
{
// lines intersect, but rays do not
riQuantity = 0;
}
}
else
{
// rays are on same line
Real fDot = kDiff.Dot(rkRay0.Direction());
if ( rkRay0.Direction().Dot(rkRay1.Direction()) > 0.0f )
{
// Rays point in same direction, get semiinfinite interval
// of intersection.
afT[0] = ( fDot >= 0.0f ? fDot/fD0SqrLen : 0.0f );
afT[1] = Math::MAX_REAL;
}
else
{
// test for overlap of opposite pointing rays
if ( fDot > 0.0f )
{
afT[0] = 0.0f;
afT[1] = fDot/fD0SqrLen;
}
else if ( fDot < 0.0f )
{
// no overlap
riQuantity = 0;
}
else
{
// rays have a common origin
riQuantity = 1;
afT[0] = 0.0f;
}
}
}
}
return riQuantity != 0;
}
void MinBox2<Real>::UpdateBox ( const Vector2<Real>& LPoint,
const Vector2<Real>& RPoint, const Vector2<Real>& BPoint,
const Vector2<Real>& TPoint, const Vector2<Real>& U,
const Vector2<Real>& V, Real& minAreaDiv4 )
{
Vector2<Real> RLDiff = RPoint - LPoint;
Vector2<Real> TBDiff = TPoint - BPoint;
Real extent0 = ( ( Real )0.5 ) * ( U.Dot( RLDiff ) );
Real extent1 = ( ( Real )0.5 ) * ( V.Dot( TBDiff ) );
Real areaDiv4 = extent0 * extent1;
if ( areaDiv4 < minAreaDiv4 )
{
minAreaDiv4 = areaDiv4;
mMinBox.Axis[0] = U;
mMinBox.Axis[1] = V;
mMinBox.Extent[0] = extent0;
mMinBox.Extent[1] = extent1;
Vector2<Real> LBDiff = LPoint - BPoint;
mMinBox.Center = LPoint + U * extent0 + V * ( extent1 - V.Dot( LBDiff ) );
}
}
float Vector2::AngleBetween(Vector2 rhs) const
{
Vector2 perp = Vector2(rhs.y, -rhs.x);
float len1 = GetLength();
float len2 = rhs.GetLength();
float angle = acos(Dot(rhs));
if (rhs.Dot(perp) > 0)
return angle;
else return -angle;
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Line2& rkLine, const Segment2& rkSegment,
int& riQuantity, Real afT[2])
{
Vector2 kDiff;
Real fD0SqrLen;
bool bIntersects = Find(rkLine.Origin(),rkLine.Direction(),
rkSegment.Origin(),rkSegment.Direction(),kDiff,fD0SqrLen,riQuantity,
afT);
if ( bIntersects )
{
if ( riQuantity == 1 )
{
if ( afT[1] < 0.0f || afT[1] > 1.0f )
{
// lines intersect, but segment does not intersect line
riQuantity = 0;
}
}
else
{
// segment is contained by line, adjust intersection interval
Real fDot = rkLine.Direction().Dot(rkSegment.Direction());
Real fInvLen = 1.0f/fD0SqrLen;
if ( fDot > 0.0f )
{
afT[0] = (kDiff.Dot(rkLine.Direction()))*fInvLen;
afT[1] = afT[0] + fDot*fInvLen;
}
else
{
afT[1] = (kDiff.Dot(rkLine.Direction()))*fInvLen;
afT[0] = afT[1] + fDot*fInvLen;
}
}
}
return riQuantity != 0;
}
// Checks intersection between a polygon an moving circle at inBegin + t * inDelta with radius^2 = inA * t^2 + inB * t + inC, t in [0, 1]
// Returns true when it does and returns the intersection position in outPoint and the intersection fraction (value for t) in outFraction
bool SweptCircleEdgeVertexIntersect(const Vector2 *inVertices, int inNumVertices, const Vector2 &inBegin, const Vector2 &inDelta, float inA, float inB, float inC, Vector2 &outPoint, float &outFraction)
{
// Loop through edges
float upper_bound = 1.0f;
bool collision = false;
for (const Vector2 *v1 = inVertices, *v2 = inVertices + inNumVertices - 1; v1 < inVertices + inNumVertices; v2 = v1, ++v1)
{
float t;
// Check if circle hits the vertex
Vector2 bv1 = *v1 - inBegin;
float a1 = inA - inDelta.GetLengthSquared();
float b1 = inB + 2.0f * inDelta.Dot(bv1);
float c1 = inC - bv1.GetLengthSquared();
if (FindLowestRootInInterval(a1, b1, c1, upper_bound, t))
{
// We have a collision
collision = true;
upper_bound = t;
outPoint = *v1;
}
// Check if circle hits the edge
Vector2 v1v2 = *v2 - *v1;
float v1v2_dot_delta = v1v2.Dot(inDelta);
float v1v2_dot_bv1 = v1v2.Dot(bv1);
float v1v2_len_sq = v1v2.GetLengthSquared();
float a2 = v1v2_len_sq * a1 + v1v2_dot_delta * v1v2_dot_delta;
float b2 = v1v2_len_sq * b1 - 2.0f * v1v2_dot_bv1 * v1v2_dot_delta;
float c2 = v1v2_len_sq * c1 + v1v2_dot_bv1 * v1v2_dot_bv1;
if (FindLowestRootInInterval(a2, b2, c2, upper_bound, t))
{
// Check if the intersection point is on the edge
float f = t * v1v2_dot_delta - v1v2_dot_bv1;
if (f >= 0.0f && f <= v1v2_len_sq)
{
// We have a collision
collision = true;
upper_bound = t;
outPoint = *v1 + v1v2 * (f / v1v2_len_sq);
}
}
}
// Check if we had a collision
if (!collision)
return false;
outFraction = upper_bound;
return true;
}
int SeparatePoints2<Real>::WhichSide (const Vector2<Real>& lineNormal,
Real lineConstant, int numEdges, const int* edges,
const Vector2<Real>* points)
{
// Establish which side of line hull is on.
Real c0;
for (int i1 = 0, i0 = numEdges-1; i1 < numEdges; i0 = i1++)
{
c0 = lineNormal.Dot(points[edges[i0]]);
if (c0 > lineConstant + Math<Real>::ZERO_TOLERANCE)
{
// Hull on positive side.
return +1;
}
if (c0 < lineConstant - Math<Real>::ZERO_TOLERANCE)
{
// Hull on negative side.
return -1;
}
c0 = lineNormal.Dot(points[edges[i1]]);
if (c0 > lineConstant + Math<Real>::ZERO_TOLERANCE)
{
// Hull on positive side.
return +1;
}
if (c0 < lineConstant - Math<Real>::ZERO_TOLERANCE)
{
// Hull on negative side.
return -1;
}
}
// Hull is effectively collinear.
return 0;
}
bool Delaunay2<Real>::InTriangle (const Vector2<Real>& rkV0,
const Vector2<Real>& rkV1, const Vector2<Real>& rkV2,
const Vector2<Real>& rkTest)
{
// test against normal to first edge
Vector2<Real> kDir = rkTest - rkV0;
Vector2<Real> kNor(rkV0.Y() - rkV1.Y(), rkV1.X() - rkV0.X());
if ( kDir.Dot(kNor) < -Math<Real>::EPSILON )
return false;
// test against normal to second edge
kDir = rkTest - rkV1;
kNor = Vector2<Real>(rkV1.Y() - rkV2.Y(), rkV2.X() - rkV1.X());
if ( kDir.Dot(kNor) < -Math<Real>::EPSILON )
return false;
// test against normal to third edge
kDir = rkTest - rkV2;
kNor = Vector2<Real>(rkV2.Y() - rkV0.Y(), rkV0.X() - rkV2.X());
if ( kDir.Dot(kNor) < -Math<Real>::EPSILON )
return false;
return true;
}
//----------------------------------------------------------------------------
bool Mgc::FindIntersection (const Line2& rkLine, const Ray2& rkRay,
int& riQuantity, Real afT[2])
{
Vector2 kDiff;
Real fD0SqrLen;
bool bIntersects = Find(rkLine.Origin(),rkLine.Direction(),
rkRay.Origin(),rkRay.Direction(),kDiff,fD0SqrLen,riQuantity,afT);
if ( bIntersects )
{
if ( riQuantity == 1 )
{
if ( afT[1] < 0.0f )
{
// lines intersect, but ray does not intersect line
riQuantity = 0;
}
}
else
{
// ray is contained by line, adjust intersection interval
if ( rkLine.Direction().Dot(rkRay.Direction()) > 0.0f )
{
afT[0] = (kDiff.Dot(rkLine.Direction()))/fD0SqrLen;
afT[1] = Math::MAX_REAL;
}
else
{
afT[0] = -Math::MAX_REAL;
afT[1] = (kDiff.Dot(rkLine.Direction()))/fD0SqrLen;
}
}
}
return riQuantity != 0;
}