C++ cVector3d类代码示例

//===========================================================================
bool cEffectStickSlip::computeForce(const cVector3d& a_toolPos,
                                  const cVector3d& a_toolVel,
                                  const unsigned int& a_toolID,
                                  cVector3d& a_reactionForce)
{
    // check if history for this IDN exists
    if (a_toolID < CHAI_EFFECT_MAX_IDN)
    {
        if (m_parent->m_interactionInside)
        {
            // check if a recent valid point has been stored previously
            if (!m_history[a_toolID].m_valid)
            {
                m_history[a_toolID].m_currentStickPosition = a_toolPos;
                m_history[a_toolID].m_valid = true;
            }

            // read parameters for stick and slip model
            double stiffness = m_parent->m_material.getStickSlipStiffness();
            double forceMax = m_parent->m_material.getStickSlipForceMax();

            // compute current force between last stick position and current tool position
            double distance = cDistance(a_toolPos, m_history[a_toolID].m_currentStickPosition);
            double forceMag = distance * stiffness;

            if (forceMag > 0)
            {
                // if force above threshold, slip...
                if (forceMag > forceMax)
                {
                    m_history[a_toolID].m_currentStickPosition = a_toolPos;
                    a_reactionForce.zero();
                }
                // ...otherwise stick
                else
                {
                    a_reactionForce = (forceMag / distance) * cSub(m_history[a_toolID].m_currentStickPosition, a_toolPos);
                }
            }
            else
            {
                a_reactionForce.zero();
            }

            return (true);
        }
        else
        {
            // the tool is located outside the object, so zero reaction force
            m_history[a_toolID].m_valid = false;
            a_reactionForce.zero();
            return (false);
        }
    }
    else
    {
        a_reactionForce.zero();
        return (false);
    }
}

//===========================================================================
bool cProxyPointForceAlgo::goalAchieved(const cVector3d& a_proxy, const cVector3d& a_goal) const
{
    if (m_useDynamicProxy)
    {
        return (!(a_proxy.distance(a_goal) > 0.0));
    }
    else
    {
        return (a_proxy.distance(a_goal) < (m_epsilonBaseValue));
    }
}

//===========================================================================
void cLight::setDir(const cVector3d& a_direction)
{

    // We arbitrarily point lights along the x axis of the stored
    // rotation matrix... this allows matrix transformations
    // to apply to lights.
    // m_localRot.setCol0(a_direction);

    cVector3d c0, c1, c2, t;
    a_direction.copyto(c0);

    // check vector
    if (c0.lengthsq() < 0.0001) { return; }

    // normalize direction vector
    c0.normalize();

    // compute 2 vector perpendicular to a_direction
    t.set(a_direction.y, a_direction.z, a_direction.x);
    t.crossr(c0, c1);
    c1.crossr(c0, c2);    

    // c0.negate();
    c1.negate();
    c2.negate();

    // update rotation matrix
    m_localRot.setCol(c0,c1,c2);
}

//===========================================================================
bool cEffectVibration::computeForce(const cVector3d& a_toolPos,
                                  const cVector3d& a_toolVel,
                                  const unsigned int& a_toolID,
                                  cVector3d& a_reactionForce)
{
    if (m_parent->m_interactionInside)
    {
        // read vibration parameters
        double vibrationFrequency = m_parent->m_material.getVibrationFrequency();
        double vibrationAmplitude = m_parent->m_material.getVibrationAmplitude();

        // read time
        double time = clock.getCurrentTimeSeconds();

        // compute force magnitude
        double forceMag = vibrationAmplitude * sin(2.0 * CHAI_PI *vibrationFrequency * time);

        a_reactionForce = cMul(forceMag, cVector3d(1, 0, 0));
        return (true);
    }
    else
    {
        // the tool is located outside the object, so zero reaction force
        a_reactionForce.zero();
        return (false);
    }
}

//===========================================================================
void cShapeSphere::computeLocalInteraction(const cVector3d& a_toolPos,
                                          const cVector3d& a_toolVel,
                                          const unsigned int a_IDN)
{
    // compute distance from center of sphere to tool
    double distance = a_toolPos.length();

    // from the position of the tool, search for the nearest point located
    // on the surface of the sphere
    if (distance > 0)
    {
        m_interactionProjectedPoint = cMul( (m_radius/distance), a_toolPos);
    }
    else
    {
        m_interactionProjectedPoint = a_toolPos;
    }

    // check if tool is located inside or outside of the sphere
    if (distance <= m_radius)
    {
        m_interactionInside = true;
    }
    else
    {
        m_interactionInside = false;
    }
}

//===========================================================================
int cVirtualDevice::getPosition(cVector3d& a_position)
{
    if (!m_systemReady)
    {
        a_position.set(0, 0, 0);
        return (-1);
    }

    double x,y,z;
    x = (double)(*m_pDevice).PosX;
    y = (double)(*m_pDevice).PosY;
    z = (double)(*m_pDevice).PosZ;
    a_position.set(x, y, z);

    return (0);
}

//==============================================================================
bool cEffectMagnet::computeForce(const cVector3d& a_toolPos,
                                 const cVector3d& a_toolVel,
                                 const unsigned int& a_toolID,
                                 cVector3d& a_reactionForce)
{
    // compute distance from object to tool
    double distance = cDistance(a_toolPos, m_parent->m_interactionPoint);

    // get parameters of magnet
    double magnetMaxForce = m_parent->m_material->getMagnetMaxForce();
    double magnetMaxDistance = m_parent->m_material->getMagnetMaxDistance();
    double stiffness = m_parent->m_material->getStiffness();
    double forceMagnitude = 0;

    if (m_enabledInside || (!m_parent->m_interactionInside))
    {
        if ((distance < magnetMaxDistance) && (stiffness > 0))
        {
            double d = magnetMaxForce / stiffness;
            if (distance < d)
            {
                forceMagnitude = stiffness * distance;
            }
            else
            {
                double dx = (magnetMaxDistance - d);
                if (dx > 0)
                {
                    double k = magnetMaxForce / dx;
                    forceMagnitude = k * (magnetMaxDistance - distance);
                }
            }

            // compute reaction force
            int sign = -1;
            if (m_parent->m_interactionInside)
            {
                sign = 1;
            }

            a_reactionForce = cMul(sign * forceMagnitude, m_parent->m_interactionNormal);

            return (true);
        }
        else
        {
            return (false);
        }
    }
    else
    {
        // the tool is located outside the magnet zone
        a_reactionForce.zero();
        return (false);
    }
}

//===========================================================================
int cPhantomDevice::getPosition(cVector3d& a_position)
{
    // check if drivers are installed
    if (!m_driverInstalled) return (-1);

    double x,y,z;
    int error = hdPhantomGetPosition(m_deviceID, &x, &y, &z);
    a_position.set(x, y, z);
    estimateLinearVelocity(a_position);
    return (error);
}

void VirtualHapticDevice::GetCursorVelocity(cVector3d& velocity) {
	double currentTime = clock.getCurrentTimeSeconds();
	double ellapsedTime = currentTime - lastClock;
	lastClock = currentTime;

	cVector3d currentPos;
	chaiDevice->getPosition(currentPos);
	velocity = currentPos - lastPosition;
	velocity.mul(1.0 / ellapsedTime );
	lastPosition.copyfrom(currentPos);
}

//==============================================================================
void cShapeTorus::computeLocalInteraction(const cVector3d& a_toolPos,
        const cVector3d& a_toolVel,
        const unsigned int a_IDN)
{
    cVector3d toolProjection = a_toolPos;
    toolProjection.z(0.0);
    m_interactionNormal.set(0,0,1);

    // search for the nearest point on the torus medial axis
    if (a_toolPos.lengthsq() > C_SMALL)
    {
        cVector3d pointAxisTorus = cMul(m_outerRadius, cNormalize(toolProjection));

        // compute eventual penetration of tool inside the torus
        cVector3d vectTorusTool = cSub(a_toolPos, pointAxisTorus);

        double distance = vectTorusTool.length();

        // normal
        if (distance > 0.0)
        {
            m_interactionNormal = vectTorusTool;
            m_interactionNormal.normalize();
        }

        // tool is located inside the torus
        if ((distance < m_innerRadius) && (distance > 0.001))
        {
            m_interactionInside = true;
        }

        // tool is located outside the torus
        else
        {
            m_interactionInside = false;
        }

        // compute surface point
        double dist = vectTorusTool.length();
        if (dist > 0)
        {
            vectTorusTool.mul(1/dist);
        }
        vectTorusTool.mul(m_innerRadius);
        pointAxisTorus.addr(vectTorusTool, m_interactionPoint);
    }
    else
    {
        m_interactionInside = false;
        m_interactionPoint = a_toolPos;
    }
}

//===========================================================================
int cVirtualDevice::getForce(cVector3d& a_force)
{
    if (!m_systemReady)
    {
        a_force.set(0,0,0);
        return (-1);
    }

    a_force.x = ((*m_pDevice).ForceX);
    a_force.y = ((*m_pDevice).ForceY);
    a_force.z = ((*m_pDevice).ForceZ);

    return (0);
}

//===========================================================================
int cHydraDevice::getPosition(cVector3d& a_position)
{
    //std::cout << "get pos";
    /************************************************************************
        STEP 7:
        Here you may implement code which reads the position (X,Y,Z) from
        your haptic device. Read the values from your device and modify
        the local variable (x,y,z) accordingly.
        If the operation fails return an error code such as -1 for instance.

        Note:
        For consistency, units must be in meters.
        If your device is located in front of you, the x-axis is pointing
        towards you (the operator). The y-axis points towards your right
        hand side and the z-axis points up towards the sky. 

    *************************************************************************/

    int error = 0;
    double x,y,z;

    // *** INSERT YOUR CODE HERE, MODIFY CODE BELLOW ACCORDINGLY ***

    sixenseAllControllerData acd;
    sixenseGetAllNewestData( &acd );


    y = acd.controllers[a_deviceNumber].pos[0] / 1000.0f;
    z = acd.controllers[a_deviceNumber].pos[1] / 1000.0f - 0.3;
    x = acd.controllers[a_deviceNumber].pos[2] / 1000.0f - 0.3;


    // offset
    cMatrix3d r;
    getRotation(r);
    cVector3d v(-0.08,0,0);
    v = r * v;




    // store new position values
    a_position.set(x+v.x(), y+v.y(), z+v.z());

    // estimate linear velocity
    estimateLinearVelocity(a_position);

    // exit
    return (error);
}

//===========================================================================
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);
}

//===========================================================================
bool cEffectViscosity::computeForce(const cVector3d& a_toolPos,
                                    const cVector3d& a_toolVel,
                                    const unsigned int& a_toolID,
                                    cVector3d& a_reactionForce)
{
    if (m_parent->m_interactionInside)
    {
        // the tool is located inside the object.
        double viscosity = m_parent->m_material.getViscosity();
        a_reactionForce = cMul(-viscosity, a_toolVel);
        return (true);
    }
    else
    {
        // the tool is located outside the object, so zero reaction force.
        a_reactionForce.zero();
        return (false);
    }
}

//===========================================================================
void cDirectionalLight::setDir(const cVector3d& a_direction)
{

    // We arbitrarily point lights along the x axis of the stored
    // rotation matrix... this allows matrix transformations
    // to apply to lights.
    // m_localRot.setCol0(a_direction);

    cVector3d c0, c1, c2, t0, t1;
    a_direction.copyto(c0);

    // check vector
    if (c0.lengthsq() < 0.0001) {
        return;
    }

    // normalize direction vector
    c0.normalize();

    // compute 2 vector perpendicular to a_direction
    t0.set(0.0, 0.0, 1.0);
    t1.set(0.0, 1.0, 0.0);
    double a0 = cAngle(c0, t0);
    double a1 = cAngle(c0, t1);

    if (sin(a0) > sin(a1))
    {
        c0.crossr(t0, c1);
        c0.crossr(c1, c2);
    }
    else
    {
        c0.crossr(t1, c1);
        c0.crossr(c1, c2);
    }

    c1.normalize();
    c2.normalize();

    // update rotation matrix
    m_localRot.setCol(c0,c1,c2);
}