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;
}
Beispiel #2
0
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;
}
Beispiel #4
0
//----------------------------------------------------------------------------
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;
 }
Beispiel #7
0
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;
}
Beispiel #9
0
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());
}
Beispiel #13
0
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);
}
Beispiel #16
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];
    }
}
Beispiel #20
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;
}
Beispiel #24
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 ) );
     }
 }
Beispiel #25
0
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;
}
Beispiel #29
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;
}