// Find the closest point on a line (or segment) to a point.
uint32_t plClosest::PointOnLine(const hsPoint3& p0,
                  const hsPoint3& p1, const hsVector3& v1,
                  hsPoint3& cp,
                  uint32_t clamp)
{
    float invV1Sq = v1.MagnitudeSquared();
    // v1 is also zero length. The two input points are the only options for output.
    if( invV1Sq < kRealSmall )
    {
        cp = p1;
        return kClamp;
    }
    float t = v1.InnerProduct(p0 - p1) / invV1Sq;
    cp = p1;
    // clamp to the ends of segment v1.
    if( (clamp & kClampLower1) && (t < 0) )
    {
        return kClampLower1;
    }
    if( (clamp & kClampUpper1) && (t > 1.f) )
    {
        cp += v1;
        return kClampUpper1;
    }

    cp += v1 * t;
    return 0;
}
hsBool plConvexVolume::BouncePoint(hsPoint3 &pos, hsVector3 &velocity, float bounce, float friction) const
{
    float minDist = 1.e33f;
    int32_t minIndex = -1;

    float currDist;
    int i;
    for (i = 0; i < fNumPlanes; i++)
    {
        currDist = -fWorldPlanes[i].fD - fWorldPlanes[i].fN.InnerProduct(pos);
        if (currDist < 0)
            return false; // We're not inside this plane, and thus outside the volume

        if (currDist < minDist)
        {
            minDist = currDist;
            minIndex = i;
        }
    }
    pos += (-fWorldPlanes[minIndex].fD - fWorldPlanes[minIndex].fN.InnerProduct(pos)) * fWorldPlanes[minIndex].fN;
    hsVector3 bnc = -velocity.InnerProduct(fWorldPlanes[minIndex].fN) * fWorldPlanes[minIndex].fN;
    velocity += bnc;
    velocity *= 1.f - friction;
    velocity += bnc * bounce;
//  velocity += (velocity.InnerProduct(fWorldPlanes[minIndex].fN) * -(1.f + bounce)) * fWorldPlanes[minIndex].fN;
    return true;
}
Exemple #3
0
bool plTriUtils::ProjectOntoPlane(const hsVector3& norm, float dist, hsPoint3& p)
{
    float normMagSq = norm.MagnitudeSquared();
    if( normMagSq > kAlmostZero )
    {
        dist /= normMagSq;

        p += norm * dist;

        return true;
    }
    return false;
}
bool plPXPhysical::GetAngularVelocitySim(hsVector3& vel) const
{
    bool result = false;
    if (fActor->isDynamic())
    {
        vel = plPXConvert::Vector(fActor->getAngularVelocity());
        result = true;
    }
    else
        vel.Set(0, 0, 0);

    return result;
}
Exemple #5
0
bool plTriUtils::ProjectOntoPlaneAlongVector(const hsVector3& norm, float dist, const hsVector3& vec, hsPoint3& p)
{
    float s = norm.InnerProduct(vec);
    const float kAlmostZero = 1.e-5f;
    if( (s > kAlmostZero)||(s < kPastZero) )
    {
        dist /= s;

        p += vec * dist;

        return true;
    }
    return false;
}
Exemple #6
0
void plSwimStraightCurrentRegion::GetCurrent(plPhysicalControllerCore *physical, hsVector3 &linearResult, float &angularResult, float elapsed)
{
    angularResult = 0.f;

    if (elapsed <= 0.f || GetProperty(kDisable))
    {
        linearResult.Set(0.f, 0.f, 0.f);
        return;
    }

    hsPoint3 center, pos;
    hsVector3 current = fCurrentSO->GetLocalToWorld() * hsVector3(0.f, 1.f, 0.f);
    hsScalarTriple xlate = fCurrentSO->GetLocalToWorld().GetTranslate();
    center.Set(&xlate);
    physical->GetPositionSim(pos);

    plKey worldKey = physical->GetSubworld();
    if (worldKey)
    {
        plSceneObject* so = plSceneObject::ConvertNoRef(worldKey->ObjectIsLoaded());
        hsMatrix44 w2l = so->GetWorldToLocal();
        center = w2l * center;
        current = w2l * current;
    }

    hsVector3 pos2Center(center.fX - pos.fX, center.fY - pos.fY, 0.f);
    float dist = current.InnerProduct(pos - center);
    float pullVel;
    
    if (dist <= fNearDist)
        pullVel = fNearVel;
    else if (dist >= fFarDist)
        pullVel = fFarVel;
    else
        pullVel = fNearVel + (fFarVel - fNearVel) * (dist - fNearDist) / (fFarDist - fNearDist);

    linearResult = current * pullVel;
}   
// Find closest points to each other from two lines (or segments).
uint32_t plClosest::PointsOnLines(const hsPoint3& p0, const hsVector3& v0, 
                  const hsPoint3& p1, const hsVector3& v1,
                  hsPoint3& cp0, hsPoint3& cp1,
                  uint32_t clamp)
{
    float invV0Sq = v0.MagnitudeSquared();
    // First handle degenerate cases.
    // v0 is zero length. Resolves to finding closest point on p1+v1 to p0
    if( invV0Sq < kRealSmall )
    {
        cp0 = p0;
        return kClamp0 | PointOnLine(p0, p1, v1, cp1, clamp);
    }
    invV0Sq = 1.f / invV0Sq;

    // The real thing here, two non-zero length segments. (v1 can
    // be zero length, it doesn't affect the math like |v0|=0 does,
    // so we don't even bother to check. Only means maybe doing extra
    // work, since we're using segment-segment math when all we really
    // need is point-segment.)

    // The parameterized points for along each of the segments are
    // P(t0) = p0 + v0*t0
    // P(t1) = p1 + v1*t1
    //
    // The closest point on p0+v0 to P(t1) is:
    //  cp0 = p0 + ((P(t1) - p0) dot v0) * v0 / ||v0||  ||x|| is mag squared here
    //  cp0 = p0 + v0*t0 => t0 = ((P(t1) - p0) dot v0 ) / ||v0||
    //                      t0 = ((p1 + v1*t1 - p0) dot v0) / ||v0||
    //
    //  The distance squared from P(t1) to cp0 is:
    //  (cp0 - P(t1)) dot (cp0 - P(t1))
    //
    //  This expands out to:
    //
    //  CV0 dot CV0 + 2 CV0 dot DV0 * t1 + (DV0 dot DV0) * t1^2
    //
    //  where
    //
    //  CV0 = p0 - p1 + ((p1 - p0) dot v0) / ||v0||) * v0 == vector from p1 to closest point on p0+v0
    //  and
    //  DV0 = ((v1 dot v0) / ||v0||) * v0 - v1 == ortho divergence vector of v1 from v0 negated.
    //
    //  Taking the first derivative to find the local minimum of the function gives
    //
    //  t1 = - (CV0 dot DV0) / (DV0 dot DV0)
    //  and
    //  t0 = ((p1 - v1 * t1 - p0) dot v0) / ||v0|| 
    //
    // which seems kind of obvious in retrospect.

    hsVector3 p0subp1(&p0, &p1);

    hsVector3 CV0 = p0subp1;
    CV0 += v0 * p0subp1.InnerProduct(v0) * -invV0Sq;
    
    hsVector3 DV0 = v0 * (v1.InnerProduct(v0) * invV0Sq) - v1;
    
    // Check for the vectors v0 and v1 being parallel, in which case
    // following the lines won't get us to any closer point.
    float DV0dotDV0 = DV0.InnerProduct(DV0);
    if( DV0dotDV0 < kRealSmall )
    {
        // If neither is clamped, return any two corresponding points.
        // If one is clamped, return closest points in its clamp range.
        // If both are clamped, well, both are clamped. The distance between
        //      points will no longer be the distance between lines.
        // In any case, the distance between the points should be correct.
        uint32_t clamp1 = PointOnLine(p0, p1, v1, cp1, clamp);
        uint32_t clamp0 = PointOnLine(cp1, p0, v0, cp0, clamp >> 1);
        return clamp1 | (clamp0 << 1);
    }
Exemple #8
0
plTriUtils::Bary plTriUtils::IComputeBarycentric(const hsVector3& v12, float invLenSq12, const hsVector3& v0, const hsVector3& v1, hsPoint3& out)
{
    uint32_t  state = 0;

    float lenSq0 = v0.MagnitudeSquared();
    if( lenSq0 < kAlmostZeroSquared )
    {
        // On edge p1-p2;
        out[0] = 0;
        state |= kOnEdge12;
    }
    else
    {
        out[0] = lenSq0 * invLenSq12;
        out[0] = sqrt(out[0]);
        // 
        if( v0.InnerProduct(v12) < 0 )
        {
            out[0] = -out[0];
            state |= kOutsideTri;
        }
        else if( out[0] > kPastOne )
            state |= kOutsideTri;
        else if( out[0] > kAlmostOne )
            state |= kOnVertex0;
    }
    
    float lenSq1 = v1.MagnitudeSquared();
    if( lenSq1 < kAlmostZeroSquared )
    {
        // On edge p0-p2
        out[1] = 0;
        state |= kOnEdge02;
    }
    else
    {
        out[1] = lenSq1 * invLenSq12;
        out[1] = sqrt(out[1]);

        if( v1.InnerProduct(v12) < 0 )
        {
            out[1] = -out[1];
            state |= kOutsideTri;
        }
        else if( out[1] > kPastOne )
            state |= kOutsideTri;
        else if( out[1] > kAlmostOne )
            state |= kOnVertex1;
    }

    // Could make more robust against precision problems
    // by repeating above for out[2], then normalizing
    // so sum(out[i]) = 1.f
    out[2] = 1.f - out[0] - out[1];

    if( out[2] < kPastZero )
        state |= kOutsideTri;
    else if( out[2] < kAlmostZero )
        state |= kOnEdge01;
    else if( out[2] > kAlmostOne )
        state |= kOnVertex2;

    /*
    if( a,b,c outside range [0..1] )
        p is outside tri;
    else if( a,b,c == 1 )
        p is on vert;
    else if( a,b,c == 0 )
        p is on edge;
    */

    if( state & kOutsideTri )
        return kOutsideTri;

    if( state & kOnVertex )
        return Bary(state & kOnVertex);

    if( state & kOnEdge )
        return Bary(state & kOnEdge);

    return kInsideTri;
}
Exemple #9
0
void plSwimRegionInterface::GetCurrent(plPhysicalControllerCore *physical, hsVector3 &linearResult, float &angularResult, float elapsed)
{
    linearResult.Set(0.f, 0.f, 0.f);
    angularResult = 0.f;
}
Exemple #10
0
void plSwimCircularCurrentRegion::GetCurrent(plPhysicalControllerCore *physical, hsVector3 &linearResult, float &angularResult, float elapsed)
{
    if (elapsed <= 0.f || fCurrentSO == nil || GetProperty(kDisable))
    {
        linearResult.Set(0.f, 0.f, 0.f);
        angularResult = 0.f;
        return;
    }

    hsPoint3 center, pos;
    hsScalarTriple xlate = fCurrentSO->GetLocalToWorld().GetTranslate();
    center.Set(&xlate);

    plKey worldKey = physical->GetSubworld();
    if (worldKey)
    {
        plSceneObject* so = plSceneObject::ConvertNoRef(worldKey->ObjectIsLoaded());
        center = so->GetWorldToLocal() * center;
    }

    center.fZ = 0.f; // Just doing 2D

    physical->GetPositionSim(pos);
    
    hsVector3 pos2Center(center.fX - pos.fX, center.fY - pos.fY, 0.f);
    float pullVel;
    float distSq = pos2Center.MagnitudeSquared();
    if (distSq < .5)
    {
        // Don't want to pull us too close to the center, or we
        // get this annoying jitter.
        pullVel = 0.f;
    }
    else if (distSq <= fPullNearDistSq)
        pullVel = fPullNearVel;
    else if (distSq >= fPullFarDistSq)
        pullVel = fPullFarVel;
    else
        pullVel = fPullNearVel + (fPullFarVel - fPullNearVel) * (distSq - fPullNearDistSq) / (fPullFarDistSq - fPullNearDistSq);

    hsVector3 pull = pos2Center;
    pull.Normalize();
    linearResult.Set(pull.fY, -pull.fX, pull.fZ);
    
    pull *= pullVel;
    linearResult *= fRotation;
    linearResult += pull;

    hsVector3 v1 = linearResult * elapsed - pos2Center;
    hsVector3 v2 = -pos2Center;
    float invCos = v1.InnerProduct(v2) / v1.Magnitude() / v2.Magnitude();
    if (invCos > 1)
        invCos = 1;
    if (invCos < -1)
        invCos = -1;
    angularResult = acos(invCos) / elapsed;

//  hsAssert(real_finite(linearResult.fX) &&
//           real_finite(linearResult.fY) &&
//           real_finite(linearResult.fZ) &&
//           real_finite(angularResult), "Bad water current computation."); 
}