C++ cVector3d::subr方法代码示例

//===========================================================================
bool cCollisionBrute::computeCollision(cVector3d& a_segmentPointA,
                                       cVector3d& a_segmentPointB,
                                       cGenericObject*& a_colObject,
                                       cTriangle*& a_colTriangle,
                                       cVector3d& a_colPoint,
                                       double& a_colSquareDistance,
                                       int a_proxyCall)
{
    // temp variables for storing results
    cGenericObject* colObject;
    cTriangle* colTriangle;
    cVector3d colPoint;
    bool hit = false;

    // convert two point segment into a segment described by a point and
    // a directional vector
    cVector3d dir;
    a_segmentPointB.subr(a_segmentPointA, dir);

    // compute the squared length of the segment
    double colSquareDistance = dir.lengthsq();

    // check all triangles for collision and return the nearest one
    unsigned int ntriangles = m_triangles->size();
    for (unsigned int i=0; i<ntriangles; i++)
    {

        // check for a collision between this triangle and the segment by
        // calling the triangle's collision detection method; it will only
        // return true if the distance between the segment origin and this
        // triangle is less than the current closest intersecting triangle
        // (whose distance squared is kept in colSquareDistance)
        if ((*m_triangles)[i].computeCollision(
            a_segmentPointA, dir, colObject, colTriangle, colPoint, colSquareDistance))
        {
            a_colObject = colObject;
            a_colTriangle = colTriangle;
            a_colPoint = colPoint;
            a_colSquareDistance = colSquareDistance;
            hit = true;
        }
    }

    // return result
    return (hit);
}

//===========================================================================
cCollisionSpheresLine::cCollisionSpheresLine(cVector3d& a_segmentPointA,
                                             cVector3d& a_segmentPointB)
{
    // calculate the center of the line segment
    m_center = cAdd(a_segmentPointA, a_segmentPointB);
    m_center.x *= 0.5;
    m_center.y *= 0.5;
    m_center.z *= 0.5;

    // calculate the radius of the bounding sphere as the distance from the
    // center of the segment (calculated above) to an endpoint
    cVector3d rad = cSub(m_center, a_segmentPointA);
    m_radius = sqrt(rad.x*rad.x + rad.y*rad.y + rad.z*rad.z);

    // set origin and direction of the line segment; i.e., redefine the segment
    // as a ray from the first endpoint (presumably the proxy position when
    // the collision detection is being used with the proxy force algorithm) to
    // the second endpoint (presumably the goal position)
    m_segmentPointA = a_segmentPointA;
    a_segmentPointB.subr(a_segmentPointA, m_dir);
}

// ch lab ---to be filled in by the students---
// Overriden from class cProxyPointForceAlgo
// Here, a_goal has been computed by collision detection above as the constrained goal towards which
// the proxy should be moved. But before moving it freely to that location, let us check if friction allows
// us to do so. we answer the question: to what extent along the proxy-goal segment can we forward the proxy?
void ch_proxyPointForceAlgo::testFrictionAndMoveProxy(const cVector3d& a_goal, const cVector3d& a_proxy, cVector3d& a_normal, cGenericObject* a_parent, const cVector3d& a_toolVel)
{

    // In this method, our aim is to calculate the the next best position for the proxy (m_nextBestProxyGlobalPos), considering friction.
    // Among other things, we are given the goal position (a_goal), the current proxy position (a_proxy), the parent object (a_parent),
    // the tool velocity (a_toolVel)


    // We will use this variable to determine if we should use the static or the dynamic
    // friction coeff to compute current frictional force.
    static double last_device_vel;



    // friction coefficients assigned to object surface
    cMesh* parent_mesh = dynamic_cast<cMesh*>(a_parent);

    // Right now we can only work with cMesh's
    if (parent_mesh == NULL)
    {
        m_nextBestProxyGlobalPos = a_goal;
        return;
    }

    // read friction coefficients here
    // -----------------------your code here------------------------------------
    double dynamic_coeff = parent_mesh->m_material.getDynamicFriction();
    double static_coeff = parent_mesh->m_material.getStaticFriction();



    // find the penetration depth of the actual device position from the nominal object surface
    double pen_depth = cDistance(a_goal, m_deviceGlobalPos);

    // shall we use the static or the dynamic friction coeff. for the cone radius calculation?
    double cone_radius;
    if(last_device_vel < SMALL_VEL)
    {
        //// the radius of the friction cone
        //-----------------------your code here------------------------------------
        cone_radius = static_coeff*pen_depth;
    }
    else
    {
        //// the radius of the friction cone
        //-----------------------your code here------------------------------------
        cone_radius = dynamic_coeff*pen_depth;
    }

    // vector from the current proxy position to the new sub-goal
    cVector3d a_proxyGoal, a_proxyGoalNormalized;
    a_goal.subr(a_proxy, a_proxyGoal);

    // normalize the proxy goal vector
    a_proxyGoal.normalizer(a_proxyGoalNormalized);

    double a_proxyGoalLength = a_proxyGoal.length();

    if(a_proxyGoalLength < cone_radius)
        // The proxy is inside the friction cone already.
        return;
    else
    {
        // The proxy is outside the friction cone, we should advance it towards the cone circumference,
        // along the vector from the current proxy position to the current goal position

        //-----------------------your code here------------------------------------
        // calculate a value for m_nextBestProxyGlobalPos
        m_nextBestProxyGlobalPos=a_proxy+(a_proxyGoalLength-cone_radius)*a_proxyGoalNormalized;
    }

    // record last velocity in order to decide if static or dynamic friction is to be applied during the
    // next iteration
    last_device_vel = a_toolVel.length();
}

//===========================================================================
bool cCollisionAABB::computeCollision(cVector3d& a_segmentPointA,
                                      cVector3d& a_segmentPointB,
                                      cGenericObject*& a_colObject,
                                      cTriangle*& a_colTriangle,
                                      cVector3d& a_colPoint,
                                      double& a_colSquareDistance,
                                      int a_proxyCall)
{
    // convert two point segment into a segment described by a point and
    // a directional vector
    cVector3d dir;
    a_segmentPointB.subr(a_segmentPointA, dir);

    // if this is a subsequent call from the proxy algorithm after detecting
    // an initial collision, and if the flag to use neighbor checking is set,
    // only neighbors of the triangle from the first collision detection
    // need to be checked
    if ((m_useNeighbors) && (a_proxyCall > 1) && (m_root != NULL) &&
        (m_lastCollision != NULL) && (m_lastCollision->m_neighbors != NULL))
    {

        // initialize temp variables for output parameters
        cGenericObject* colObject;
        cTriangle* colTriangle;
        cVector3d colPoint;
        double colSquareDistance = dir.lengthsq();
        bool firstHit = true;

        // check each neighbor, and find the closest for which there is a
        // collision, if any
        unsigned int ntris = m_lastCollision->m_neighbors->size();
        std::vector<cTriangle*>* neighbors = m_lastCollision->m_neighbors;
        for (unsigned int i=0; i<ntris; i++)
        {
            cTriangle* tri = (*neighbors)[i];
            if (tri == 0) {
              CHAI_DEBUG_PRINT("Oops... invalid neighbor\n");
              continue;
            }
            if (tri->computeCollision(
                    a_segmentPointA, dir, colObject, colTriangle,
                    colPoint, colSquareDistance))
            {
                // if this intersected triangle is closer to the segment origin
                // than any other found so far, set the output parameters
                if (firstHit || (colSquareDistance < a_colSquareDistance))
                {
                    m_lastCollision = colTriangle;
                    a_colObject = colObject;
                    a_colTriangle = colTriangle;
                    a_colPoint = colPoint;
                    a_colSquareDistance = colSquareDistance;
                    firstHit = false;
                }
            }
        }

        // if at least one neighbor triangle was intersected, return true
        if (!firstHit)  return true;

        // otherwise there was no collision; return false
        if (a_proxyCall != -1) m_lastCollision = NULL;
        return false;
    }

    // otherwise, if this is the first call in an iteration of the proxy
    // algorithm (or a call from any other algorithm), check the AABB tree

    // if the root is null, the tree is empty, so there can be no collision
    if (m_root == NULL)
    {
        if (a_proxyCall != -1) m_lastCollision = NULL;
        return (false);
    }

    // create an axis-aligned bounding box for the line
    cCollisionAABBBox lineBox;
    lineBox.setEmpty();
    lineBox.enclose(a_segmentPointA);
    lineBox.enclose(a_segmentPointB);

    // test for intersection between the line segment and the root of the
    // collision tree; the root will recursively call children down the tree
    a_colSquareDistance = dir.lengthsq();
    bool result = m_root->computeCollision(a_segmentPointA, dir, lineBox,
            a_colTriangle, a_colPoint, a_colSquareDistance);

    // if there was a collision, set m_lastCollision to the intersected triangle
    // returned by the call to the root of the tree, and set the output
    // parameter for the intersected mesh to the parent of this triangle
    if (result)
    {
        if (a_proxyCall != -1) m_lastCollision = a_colTriangle;
        a_colObject = a_colTriangle->getParent();
    }
    else
    {
        if (a_proxyCall != -1) m_lastCollision = NULL;
    }
//.........这里部分代码省略.........

//==============================================================================
bool cTriangleArray::computeCollision(const unsigned int a_triangleIndex,
                                      cGenericObject* a_object,
                                      cVector3d& a_segmentPointA,
                                      cVector3d& a_segmentPointB,
                                      cCollisionRecorder& a_recorder,
                                      cCollisionSettings& a_settings) const
{
    // verify that triangle is active
    if (!m_allocated[a_triangleIndex]) { return (false); }

    // temp variables
    bool hit = false;
    cVector3d collisionPoint;
    cVector3d collisionNormal;
    double collisionDistanceSq = C_LARGE;
    double collisionPointV01 = 0.0;
    double collisionPointV02 = 0.0;

    // retrieve information about which side of the triangles need to be checked
    cMaterialPtr material = a_object->m_material;
    bool checkFrontSide = material->getHapticTriangleFrontSide();
    bool checkBackSide = material->getHapticTriangleBackSide();

    // retrieve vertex positions
    cVector3d vertex0 = m_vertices->getLocalPos(getVertexIndex0(a_triangleIndex));
    cVector3d vertex1 = m_vertices->getLocalPos(getVertexIndex1(a_triangleIndex));
    cVector3d vertex2 = m_vertices->getLocalPos(getVertexIndex2(a_triangleIndex));

    // If m_collisionRadius == 0, we search for a possible intersection between
    // the segment AB and the triangle defined by its three vertices V0, V1, V2.
    if (a_settings.m_collisionRadius == 0.0)
    {
        // check for collision between segment and triangle only
        if (cIntersectionSegmentTriangle(a_segmentPointA,
                                         a_segmentPointB,
                                         vertex0,
                                         vertex1,
                                         vertex2,
                                         checkFrontSide,
                                         checkBackSide,
                                         collisionPoint,
                                         collisionNormal,
                                         collisionPointV01,
                                         collisionPointV02))
        {
            hit = true;
            collisionDistanceSq = cDistanceSq(a_segmentPointA, collisionPoint);
        }
    }


    // If m_collisionRadius > 0, we search for a possible intersection between
    // the segment AB and the shell of the selected triangle which is described
    // by its three vertices and m_collisionRadius.
    else
    {
        cVector3d t_collisionPoint, t_collisionNormal;
        double t_collisionDistanceSq;
        cVector3d normal = cComputeSurfaceNormal(vertex0, vertex1, vertex2);
        cVector3d offset; normal.mulr(a_settings.m_collisionRadius, offset);
        cVector3d t_vertex0, t_vertex1, t_vertex2;
        double t_collisionPointV01, t_collisionPointV02, t_collisionPointV12;

        // check for collision between segment and triangle upper shell
        vertex0.addr(offset, t_vertex0);
        vertex1.addr(offset, t_vertex1);
        vertex2.addr(offset, t_vertex2);
        if (cIntersectionSegmentTriangle(a_segmentPointA,
                                         a_segmentPointB,
                                         t_vertex0,
                                         t_vertex1,
                                         t_vertex2,
                                         checkFrontSide,
                                         false,
                                         collisionPoint,
                                         collisionNormal,
                                         collisionPointV01,
                                         collisionPointV02))
        {
            hit = true;
            collisionDistanceSq = cDistanceSq(a_segmentPointA, collisionPoint);
        }

        // check for collision between segment and triangle lower shell
        vertex0.subr(offset, t_vertex0);
        vertex1.subr(offset, t_vertex1);
        vertex2.subr(offset, t_vertex2);
        if (cIntersectionSegmentTriangle(a_segmentPointA,
                                         a_segmentPointB,
                                         t_vertex0,
                                         t_vertex1,
                                         t_vertex2,
                                         false,
                                         checkBackSide,
                                         t_collisionPoint,
                                         t_collisionNormal,
                                         t_collisionPointV01,
                                         t_collisionPointV02))
        {
//.........这里部分代码省略.........