Method CreateFullControl

ogsr_engine/xrGame/PHJoint.cpp:305–408

#define FIX_BY_ONE_HINGE
#endif
void CPHJoint::CreateFullControl()
{
    Fvector pos;
    Fmatrix first_matrix, second_matrix;
    Fvector axis;
    CPHElement* first = (pFirst_element);
    CPHElement* second = (pSecond_element);
    VERIFY(first);
    first->GetGlobalTransformDynamic(&first_matrix);
    dBodyID body1 = body_for_joint(first);
    VERIFY(second);
    second->GetGlobalTransformDynamic(&second_matrix);
    dBodyID body2 = body_for_joint(second);

    pos.set(0, 0, 0);
    switch (vs_anchor)
    {
    case vs_first: first_matrix.transform_tiny(pos, anchor); break;
    case vs_second: second_matrix.transform_tiny(pos, anchor); break;
    case vs_global: pShell->mXFORM.transform_tiny(pos, anchor); break;
    default: NODEFAULT;
    }
    //////////////////////////////////////

    m_joint = dJointCreateBall(0, 0);
    dJointAttach(m_joint, body1, body2);
    dJointSetBallAnchor(m_joint, pos.x, pos.y, pos.z);

    m_joint1 = dJointCreateAMotor(0, 0);
    dJointSetAMotorMode(m_joint1, dAMotorEuler);
    dJointSetAMotorNumAxes(m_joint1, 3);

    dJointAttach(m_joint1, body1, body2);

    /////////////////////////////////////////////

    Fmatrix first_matrix_inv;
    first_matrix_inv.set(first_matrix);
    first_matrix_inv.invert();
    Fmatrix rotate;
    rotate.mul(first_matrix_inv, second_matrix);
    /////////////////////////////////////////////

    float lo;
    float hi;
    //////////////////////////////////////////////////////////
    /////////////////////////////////////////////////////////
    axis.set(0, 0, 0);
    // axis 0
    CalcAxis(0, axis, lo, hi, first_matrix, second_matrix, rotate);
    if (!body1)
        axis.invert(); // SwapLimits(lo,hi);
    dJointSetAMotorAxis(m_joint1, 0, 1, axis.x, axis.y, axis.z);
    dJointSetAMotorParam(m_joint1, dParamLoStop, lo);
    dJointSetAMotorParam(m_joint1, dParamHiStop, hi);

    if (!(axes[0].force < 0.f))
    {