joint_pserocka.cpp

赫赫大名的 OGRE 游戏引擎

/*
    This code is part of the Plane2D ODE joint
    by [e-mail]
    Wed Apr 23 18:53:43 CEST 2003
    
    Add this code to the file: ode/src/joint.cpp
*/


# define        VoXYZ(v1, o1, x, y, z) \
                    ( \
                        (v1)[0] o1 (x), \
                        (v1)[1] o1 (y), \
                        (v1)[2] o1 (z)  \
                    )

static dReal   Midentity[3][3] =
                {
                    {   1,  0,  0   },
                    {   0,  1,  0   },
                    {   0,  0,  1,  }
                };



static void     plane2dInit (dxJointPlane2D *j)
/*********************************************/
{
    /* MINFO ("plane2dInit ()"); */
    j->motor_x.init (j->world);
    j->motor_y.init (j->world);
    j->motor_angle.init (j->world);
}



static void     plane2dGetInfo1 (dxJointPlane2D *j, dxJoint::Info1 *info)
/***********************************************************************/
{
  /* MINFO ("plane2dGetInfo1 ()"); */

  info->nub = 3;
  info->m = 3;

  if (j->motor_x.fmax > 0)
      j->row_motor_x = info->m ++;
  if (j->motor_y.fmax > 0)
      j->row_motor_y = info->m ++;
  if (j->motor_angle.fmax > 0)
      j->row_motor_angle = info->m ++;
}



static void     plane2dGetInfo2 (dxJointPlane2D *joint, dxJoint::Info2 *info)
/***************************************************************************/
{
    int         r0 = 0,
                r1 = info->rowskip,
                r2 = 2 * r1;
    dReal       eps = info->fps * info->erp;

    /* MINFO ("plane2dGetInfo2 ()"); */

/*
    v = v1, w = omega1
    (v2, omega2 not important (== static environment))

    constraint equations:
        xz = 0
        wx = 0
        wy = 0

    <=> ( 0 0 1 ) (vx)   ( 0 0 0 ) (wx)   ( 0 )
        ( 0 0 0 ) (vy) + ( 1 0 0 ) (wy) = ( 0 )
        ( 0 0 0 ) (vz)   ( 0 1 0 ) (wz)   ( 0 )
        J1/J1l           Omega1/J1a
*/

    // fill in linear and angular coeff. for left hand side:

    VoXYZ (&info->J1l[r0], =, 0, 0, 1);
    VoXYZ (&info->J1l[r1], =, 0, 0, 0);
    VoXYZ (&info->J1l[r2], =, 0, 0, 0);

    VoXYZ (&info->J1a[r0], =, 0, 0, 0);
    VoXYZ (&info->J1a[r1], =, 1, 0, 0);
    VoXYZ (&info->J1a[r2], =, 0, 1, 0);

    // error correction (against drift):

    // a) linear vz, so that z (== pos[2]) == 0
    info->c[0] = eps * -joint->node[0].body->pos[2];

# if 0
    // b) angular correction? -> left to application !!!
    dReal       *body_z_axis = &joint->node[0].body->R[8];
    info->c[1] = eps * +atan2 (body_z_axis[1], body_z_axis[2]); // wx error
    info->c[2] = eps * -atan2 (body_z_axis[0], body_z_axis[2]); // wy error
# endif

    // if the slider is powered, or has joint limits, add in the extra row:

    if (joint->row_motor_x > 0)
        joint->motor_x.addLimot (
            joint, info, joint->row_motor_x, Midentity[0], 0);

    if (joint->row_motor_y > 0)
        joint->motor_y.addLimot (
            joint, info, joint->row_motor_y, Midentity[1], 0);

    if (joint->row_motor_angle > 0)
        joint->motor_angle.addLimot (
            joint, info, joint->row_motor_angle, Midentity[2], 1);
}



dxJoint::Vtable __dplane2d_vtable =
{
  sizeof (dxJointPlane2D),
  (dxJoint::init_fn*) plane2dInit,
  (dxJoint::getInfo1_fn*) plane2dGetInfo1,
  (dxJoint::getInfo2_fn*) plane2dGetInfo2,
  dJointTypePlane2D
};


extern "C" void dJointSetPlane2DXParam (dxJointPlane2D *joint,
                      int parameter, dReal value)
{
  dUASSERT (joint, "bad joint argument");
  dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d");
  joint->motor_x.set (parameter, value);
}


extern "C" void dJointSetPlane2DYParam (dxJointPlane2D *joint,
                      int parameter, dReal value)
{
  dUASSERT (joint, "bad joint argument");
  dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d");
  joint->motor_y.set (parameter, value);
}



extern "C" void dJointSetPlane2DAngleParam (dxJointPlane2D *joint,
                      int parameter, dReal value)
{
  dUASSERT (joint, "bad joint argument");
  dUASSERT (joint->vtable == &__dplane2d_vtable, "joint is not a plane2d");
  joint->motor_angle.set (parameter, value);
}