C++ dNormalize3函数代码示例

static int edgeIntersectsRect (dVector3 v1, dVector3 v2,
			       dVector3 p1, dVector3 p2, dVector3 p3)
{
  int k;
  dVector3 u1,u2,n,tmp;
  for (k=0; k<3; k++) u1[k] = p3[k]-p1[k];
  for (k=0; k<3; k++) u2[k] = p2[k]-p1[k];
  dReal d1 = dSqrt(dCalcVectorDot3(u1,u1));
  dReal d2 = dSqrt(dCalcVectorDot3(u2,u2));
  dNormalize3 (u1);
  dNormalize3 (u2);
  if (dFabs(dCalcVectorDot3(u1,u2)) > 1e-6) dDebug (0,"bad u1/u2");
  dCalcVectorCross3(n,u1,u2);
  for (k=0; k<3; k++) tmp[k] = v2[k]-v1[k];
  dReal d = -dCalcVectorDot3(n,p1);
  if (dFabs(dCalcVectorDot3(n,p1)+d) > 1e-8) dDebug (0,"bad n wrt p1");
  if (dFabs(dCalcVectorDot3(n,p2)+d) > 1e-8) dDebug (0,"bad n wrt p2");
  if (dFabs(dCalcVectorDot3(n,p3)+d) > 1e-8) dDebug (0,"bad n wrt p3");
  dReal alpha = -(d+dCalcVectorDot3(n,v1))/dCalcVectorDot3(n,tmp);
  for (k=0; k<3; k++) tmp[k] = v1[k]+alpha*(v2[k]-v1[k]);
  if (dFabs(dCalcVectorDot3(n,tmp)+d) > 1e-6) dDebug (0,"bad tmp");
  if (alpha < 0) return 0;
  if (alpha > 1) return 0;
  for (k=0; k<3; k++) tmp[k] -= p1[k];
  dReal a1 = dCalcVectorDot3(u1,tmp);
  dReal a2 = dCalcVectorDot3(u2,tmp);
  if (a1<0 || a2<0 || a1>d1 || a2>d2) return 0;
  return 1;
}

void makeRandomRotation (dMatrix3 R)
{
  dReal *u1 = R, *u2=R+4, *u3=R+8;
  dMakeRandomVector (u1,3,1.0);
  dNormalize3 (u1);
  dMakeRandomVector (u2,3,1.0);
  dReal d = dCalcVectorDot3(u1,u2);
  u2[0] -= d*u1[0];
  u2[1] -= d*u1[1];
  u2[2] -= d*u1[2];
  dNormalize3(u2);
  dCalcVectorCross3(u3,u1,u2);
}

void
dxJointHinge2::makeW1andW2()
{
    if ( node[1].body )
    {
        // get axis 1 and 2 in global coords
        dVector3 ax1, ax2, w;
        dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
        dMultiply0_331( ax2, node[1].body->posr.R, axis2 );

        // don't do anything if the axis1 or axis2 vectors are zero or the same
        if (( ax1[0] == 0 && ax1[1] == 0 && ax1[2] == 0 ) ||
            ( ax2[0] == 0 && ax2[1] == 0 && ax2[2] == 0 ) ||
            ( ax1[0] == ax2[0] && ax1[1] == ax2[1] && ax1[2] == ax2[2] ) ) return;

        // modify axis 1 so it's perpendicular to axis 2
        dReal k = dCalcVectorDot3( ax2, ax1 );
        for ( int i = 0; i < 3; i++ ) ax1[i] -= k * ax2[i];
        dNormalize3( ax1 );

        // make w1 = modified axis1, w2 = axis2 x (modified axis1)
        dCalcVectorCross3( w, ax2, ax1 );
        dMultiply1_331( w1, node[1].body->posr.R, ax1 );
        dMultiply1_331( w2, node[1].body->posr.R, w );
    }
}

//plane plane
bool FindIntersectionPlanePlane(const dReal Plane0[4], const dReal Plane1[4],
	dVector3 LinePos,dVector3 LineDir)
{
    // If Cross(N0,N1) is zero, then either planes are parallel and separated
    // or the same plane.  In both cases, 'false' is returned.  Otherwise,
    // the intersection line is
    //
    //   L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1
    //
    // for some coefficients c0 and c1 and for t any real number (the line
    // parameter).  Taking dot products with the normals,
    //
    //   d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1)
    //   d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1)
    //
    // which are two equations in two unknowns.  The solution is
    //
    //   c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det
    //   c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det
    //
    // where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2.
/*
    Real fN00 = rkPlane0.Normal().SquaredLength();
    Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal());
    Real fN11 = rkPlane1.Normal().SquaredLength();
    Real fDet = fN00*fN11 - fN01*fN01;

    if ( Math::FAbs(fDet) < gs_fEpsilon )
        return false;

    Real fInvDet = 1.0f/fDet;
    Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet;
    Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet;

    rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal());
    rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal();
    return true;
*/
	dReal fN00 = dLENGTHSQUARED(Plane0);
    dReal fN01 = dDOT(Plane0,Plane1);
    dReal fN11 = dLENGTHSQUARED(Plane1);
    dReal fDet = fN00*fN11 - fN01*fN01;

    if ( fabs(fDet) < fEPSILON)
        return false;

    dReal fInvDet = 1.0f/fDet;
    dReal fC0 = (fN11*Plane0[3] - fN01*Plane1[3])*fInvDet;
    dReal fC1 = (fN00*Plane1[3] - fN01*Plane0[3])*fInvDet;

    dCROSS(LineDir,=,Plane0,Plane1);
	dNormalize3(LineDir);

	dVector3 Temp0,Temp1;
	dOPC(Temp0,*,Plane0,fC0);
	dOPC(Temp1,*,Plane1,fC1);
	dOP(LinePos,+,Temp0,Temp1);

    return true;
}

void
dxJointHinge2::makeV1andV2()
{
    if ( node[0].body )
    {
        // get axis 1 and 2 in global coords
        dVector3 ax1, ax2, v;
        dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
        dMultiply0_331( ax2, node[1].body->posr.R, axis2 );

        // don't do anything if the axis1 or axis2 vectors are zero or the same
        if ((_dequal(ax1[0], 0.0) && _dequal(ax1[1], 0.0) && _dequal(ax1[2], 0.0)) ||
            (_dequal(ax2[0], 0.0) && _dequal(ax2[1], 0.0) && _dequal(ax2[2], 0.0)) ||
            (_dequal(ax1[0], ax2[0]) && _dequal(ax1[1], ax2[1]) && _dequal(ax1[2], ax2[2])))
          return;

        // modify axis 2 so it's perpendicular to axis 1
        dReal k = dCalcVectorDot3( ax1, ax2 );
        for ( int i = 0; i < 3; i++ ) ax2[i] -= k * ax1[i];
        dNormalize3( ax2 );

        // make v1 = modified axis2, v2 = axis1 x (modified axis2)
        dCalcVectorCross3( v, ax1, ax2 );
        dMultiply1_331( v1, node[0].body->posr.R, ax2 );
        dMultiply1_331( v2, node[0].body->posr.R, v );
    }
}

void setAxes( dxJoint *j, dReal x, dReal y, dReal z,
              dVector3 axis1, dVector3 axis2 )
{
    if ( j->node[0].body )
    {
        dReal q[4];
        q[0] = x;
        q[1] = y;
        q[2] = z;
        q[3] = 0;
        dNormalize3( q );
        if ( axis1 )
        {
            dMultiply1_331( axis1, j->node[0].body->posr.R, q );
            axis1[3] = 0;
        }
        if ( axis2 )
        {
            if ( j->node[1].body )
            {
                dMultiply1_331( axis2, j->node[1].body->posr.R, q );
            }
            else
            {
                axis2[0] = x;
                axis2[1] = y;
                axis2[2] = z;
            }
            axis2[3] = 0;
        }
    }
}

void fixFrictionVector( dBodyID b1, dBodyID b2, dContact& contact )
{
	dBodyGetPointVel(b1, contact.geom.pos[0], contact.geom.pos[1], contact.geom.pos[2], contact.fdir1);
	dVector3 fdir1_b2;
	if (b2)
	{
		dBodyGetPointVel(b2, contact.geom.pos[0], contact.geom.pos[1],	contact.geom.pos[2], fdir1_b2);
		contact.fdir1[0] -= fdir1_b2[0];
		contact.fdir1[1] -= fdir1_b2[1];
		contact.fdir1[2] -= fdir1_b2[2];
	}
	// at this point, contact[i].fdir1 is the relative tangent velocity of the two bodies.
	dCROSS(contact.fdir1, =, contact.fdir1, contact.geom.normal);
	// now, contact[i].fdir1 is perpendicular to both the normal and
	// the relative tangent velocity.
	double length = sqrt(contact.fdir1[0] * contact.fdir1[0]
	       			   + contact.fdir1[1] * contact.fdir1[1]
				       + contact.fdir1[2] * contact.fdir1[2]);
	if (length > 1e-12)
	{
	       // we only use our calculated direction if it has enough precision
	       contact.fdir1[0] /= length;
	       contact.fdir1[1] /= length;
	       contact.fdir1[2] /= length;
	       dNormalize3(contact.fdir1);
	       contact.surface.mode |= dContactFDir1;
	}

}

void dJointSetGearboxAxis2( dJointID j, dReal x, dReal y, dReal z )
{
    dxJointGearbox* joint = static_cast<dxJointGearbox*>(j);
    dUASSERT( joint, "bad joint argument" );

    dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axis2);
    dNormalize3(joint->axis2);
}

int test_sphere_point_depth()
{
  int j;
  dVector3 p,q;
  dMatrix3 R;
  dReal r,d;

  dSimpleSpace space(0);
  dGeomID sphere = dCreateSphere (0,1);
  dSpaceAdd (space,sphere);

  // ********** make a random sphere of radius r at position p

  r = dRandReal()+0.1;
  dGeomSphereSetRadius (sphere,r);
  dMakeRandomVector (p,3,1.0);
  dGeomSetPosition (sphere,p[0],p[1],p[2]);
  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
		      dRandReal()*2-1,dRandReal()*10-5);
  dGeomSetRotation (sphere,R);

  // ********** test center point has depth r

  if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED();

  // ********** test point on surface has depth 0

  for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
  dNormalize3 (q);
  for (j=0; j<3; j++) q[j] = q[j]*r + p[j];
  if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED();

  // ********** test point at random depth

  d = (dRandReal()*2-1) * r;
  for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
  dNormalize3 (q);
  for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j];
  if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED();

  PASSED();
}

// compute the 3 axes in global coordinates
void
dxJointAMotor::computeGlobalAxes( dVector3 ax[3] )
{
    if ( mode == dAMotorEuler )
    {
        // special handling for euler mode
        dMultiply0_331( ax[0], node[0].body->posr.R, axis[0] );
        if ( node[1].body )
        {
            dMultiply0_331( ax[2], node[1].body->posr.R, axis[2] );
        }
        else
        {
            ax[2][0] = axis[2][0];
            ax[2][1] = axis[2][1];
            ax[2][2] = axis[2][2];
        }
        dCalcVectorCross3( ax[1], ax[2], ax[0] );
        dNormalize3( ax[1] );
    }
    else
    {
        for ( int i = 0; i < num; i++ )
        {
            if ( rel[i] == 1 )
            {
                // relative to b1
                dMultiply0_331( ax[i], node[0].body->posr.R, axis[i] );
            }
            else if ( rel[i] == 2 )
            {
                // relative to b2
                if ( node[1].body )   // jds: don't assert, just ignore
                {
                    dMultiply0_331( ax[i], node[1].body->posr.R, axis[i] );
                }
                else
                {
                    // global - just copy it
                    ax[i][0] = axis[i][0];
                    ax[i][1] = axis[i][1];
                    ax[i][2] = axis[i][2];
                }
            }
            else
            {
                // global - just copy it
                ax[i][0] = axis[i][0];
                ax[i][1] = axis[i][1];
                ax[i][2] = axis[i][2];
            }
        }
    }
}

void
dxJointHinge2::getInfo2( dReal worldFPS, dReal worldERP, const Info2Descr *info )
{
    // get information we need to set the hinge row
    dReal s, c;
    dVector3 q;
    const dxJointHinge2 *joint = this;

    dVector3 ax1, ax2;
    joint->getAxisInfo( ax1, ax2, q, s, c );
    dNormalize3( q );   // @@@ quicker: divide q by s ?

    // set the three ball-and-socket rows (aligned to the suspension axis ax1)
    setBall2( this, worldFPS, worldERP, info, anchor1, anchor2, ax1, susp_erp );

    // set the hinge row
    int s3 = 3 * info->rowskip;
    info->J1a[s3+0] = q[0];
    info->J1a[s3+1] = q[1];
    info->J1a[s3+2] = q[2];
    if ( joint->node[1].body )
    {
        info->J2a[s3+0] = -q[0];
        info->J2a[s3+1] = -q[1];
        info->J2a[s3+2] = -q[2];
    }

    // compute the right hand side for the constrained rotational DOF.
    // axis 1 and axis 2 are separated by an angle `theta'. the desired
    // separation angle is theta0. sin(theta0) and cos(theta0) are recorded
    // in the joint structure. the correcting angular velocity is:
    //   |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize
    //                      = (erp*fps) * (theta0-theta)
    // (theta0-theta) can be computed using the following small-angle-difference
    // approximation:
    //   theta0-theta ~= tan(theta0-theta)
    //                 = sin(theta0-theta)/cos(theta0-theta)
    //                 = (c*s0 - s*c0) / (c*c0 + s*s0)
    //                 = c*s0 - s*c0         assuming c*c0 + s*s0 ~= 1
    // where c = cos(theta), s = sin(theta)
    //       c0 = cos(theta0), s0 = sin(theta0)

    dReal k = worldFPS * worldERP;
    info->c[3] = k * ( c0 * s - joint->s0 * c );

    // if the axis1 hinge is powered, or has joint limits, add in more stuff
    int row = 4 + limot1.addLimot( this, worldFPS, info, 4, ax1, 1 );

    // if the axis2 hinge is powered, add in more stuff
    limot2.addLimot( this, worldFPS, info, row, ax2, 1 );

    // set parameter for the suspension
    info->cfm[0] = susp_cfm;
}

void dJointSetAMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal z )
{
    dxJointAMotor* joint = ( dxJointAMotor* )j;
    dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 );
    checktype( joint, AMotor );
    dUASSERT( !( !joint->node[1].body && ( joint->flags & dJOINT_REVERSE ) && rel == 1 ), "no first body, can't set axis rel=1" );
    dUASSERT( !( !joint->node[1].body && !( joint->flags & dJOINT_REVERSE ) && rel == 2 ), "no second body, can't set axis rel=2" );
    if ( anum < 0 ) anum = 0;
    if ( anum > 2 ) anum = 2;

    // adjust rel to match the internal body order
    if ( !joint->node[1].body && rel == 2 ) rel = 1;

    joint->rel[anum] = rel;

    // x,y,z is always in global coordinates regardless of rel, so we may have
    // to convert it to be relative to a body
    dVector3 r;
    r[0] = x;
    r[1] = y;
    r[2] = z;
    r[3] = 0;
    if ( rel > 0 )
    {
        if ( rel == 1 )
        {
            dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r );
        }
        else
        {
            // don't assert; handle the case of attachment to a bodiless geom
            if ( joint->node[1].body )   // jds
            {
                dMultiply1_331( joint->axis[anum], joint->node[1].body->posr.R, r );
            }
            else
            {
                joint->axis[anum][0] = r[0];
                joint->axis[anum][1] = r[1];
                joint->axis[anum][2] = r[2];
                joint->axis[anum][3] = r[3];
            }
        }
    }
    else
    {
        joint->axis[anum][0] = r[0];
        joint->axis[anum][1] = r[1];
        joint->axis[anum][2] = r[2];
    }
    dNormalize3( joint->axis[anum] );
    if ( joint->mode == dAMotorEuler ) joint->setEulerReferenceVectors();
}

void dJointPlanarSetPlaneNormal(dJointID joint, dVector3 planeNormal) {
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, Plane2D );
    dxPlanarJoint* planarJoint = (dxPlanarJoint*) joint;

    dUASSERT(dLENGTHSQUARED(planeNormal) > 0.000001, "plane normal cannot have zero length");

    dCopyVector3(planarJoint->planeNormal, planeNormal);
    dNormalize3(planarJoint->planeNormal);

    planarJoint->updatePlane();
}

void dJointSetTransmissionAxis2( dJointID j, dReal x, dReal y, dReal z )
{    
    dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
    dUASSERT( joint, "bad joint argument" );
    dUASSERT(joint->mode = dTransmissionIntersectingAxes,
             "can't set individual axes in current mode" );

    if (joint->node[1].body) {
        dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axes[1]);
        dNormalize3(joint->axes[1]);
    }
    
    joint->update = 1;
}

void testPlaneSpace()
{
  HEADER;
  dVector3 n,p,q;
  int bad = 0;
  for (int i=0; i<1000; i++) {
    dMakeRandomVector (n,3,1.0);
    dNormalize3 (n);
    dPlaneSpace (n,p,q);
    if (fabs(dCalcVectorDot3(n,p)) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(n,q)) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(p,q)) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(p,p)-1) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(q,q)-1) > tol) bad = 1;
  }
  printf ("\t%s\n", bad ? "FAILED" : "passed");
}