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; }