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
dxJointAMotor::setEulerReferenceVectors()
{
    if ( node[0].body && node[1].body )
    {
        dVector3 r;  // axis[2] and axis[0] in global coordinates
        dMultiply0_331( r, node[1].body->posr.R, axis[2] );
        dMultiply1_331( reference1, node[0].body->posr.R, r );
        dMultiply0_331( r, node[0].body->posr.R, axis[0] );
        dMultiply1_331( reference2, node[1].body->posr.R, r );
    }

    else     // jds
    {
        // else if (j->node[0].body) {
        // dMultiply1_331 (j->reference1,j->node[0].body->posr.R,j->axis[2]);
        // dMultiply0_331 (j->reference2,j->node[0].body->posr.R,j->axis[0]);

        // We want to handle angular motors attached to passive geoms
        dVector3 r;  // axis[2] and axis[0] in global coordinates
        r[0] = axis[2][0];
        r[1] = axis[2][1];
        r[2] = axis[2][2];
        r[3] = axis[2][3];
        dMultiply1_331( reference1, node[0].body->posr.R, r );
        dMultiply0_331( r, node[0].body->posr.R, axis[0] );
        reference2[0] += r[0];
        reference2[1] += r[1];
        reference2[2] += r[2];
        reference2[3] += r[3];
    }
}

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

void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
{
    dxJointAMotor* joint = ( dxJointAMotor* )j;
    dAASSERT( joint && anum >= 0 && anum < 3 );
    checktype( joint, AMotor );
    if ( anum < 0 ) anum = 0;
    if ( anum > 2 ) anum = 2;
    if ( joint->rel[anum] > 0 )
    {
        if ( joint->rel[anum] == 1 )
        {
            dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
        }
        else
        {
            if ( joint->node[1].body )   // jds
            {
                dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis[anum] );
            }
            else
            {
                result[0] = joint->axis[anum][0];
                result[1] = joint->axis[anum][1];
                result[2] = joint->axis[anum][2];
                result[3] = joint->axis[anum][3];
            }
        }
    }
    else
    {
        result[0] = joint->axis[anum][0];
        result[1] = joint->axis[anum][1];
        result[2] = joint->axis[anum][2];
    }
}

void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
{
    dxJointAMotor* joint = ( dxJointAMotor* )j;
    dAASSERT( joint && anum >= 0 && anum < 3 );
    checktype( joint, AMotor );
    if ( anum < 0 ) anum = 0;
    if ( anum > 2 ) anum = 2;
    
    // If we're in Euler mode, joint->axis[1] doesn't
    // have anything sensible in it.  So don't just return
    // that, find the actual effective axis.
    // Likewise, the actual axis of rotation for the
    // the other axes is different from what's stored.
    if ( joint->mode == dAMotorEuler  ) {
      dVector3 axes[3];
      joint->computeGlobalAxes(axes);
      if (anum == 1) {
        result[0]=axes[1][0];
        result[1]=axes[1][1];
        result[2]=axes[1][2];
      } else if (anum == 0) {
        // This won't be unit length in general,
        // but it's what's used in getInfo2
        // This may be why things freak out as
        // the body-relative axes get close to each other.
        dCalcVectorCross3( result, axes[1], axes[2] );
      } else if (anum == 2) {
        // Same problem as above.
        dCalcVectorCross3( result, axes[0], axes[1] );
      }
    } else if ( joint->rel[anum] > 0 ) {
        if ( joint->rel[anum] == 1 )
        {
            dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
        }
        else
        {
            if ( joint->node[1].body )   // jds
            {
                dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis[anum] );
            }
            else
            {
                result[0] = joint->axis[anum][0];
                result[1] = joint->axis[anum][1];
                result[2] = joint->axis[anum][2];
                result[3] = joint->axis[anum][3];
            }
        }
    }
    else
    {
        result[0] = joint->axis[anum][0];
        result[1] = joint->axis[anum][1];
        result[2] = joint->axis[anum][2];
    }
}

// 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];
            }
        }
    }
}

dReal dJointGetPRPosition( dJointID j )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PR );

    dVector3 q;
    // get the offset in global coordinates
    dMultiply0_331( q, joint->node[0].body->posr.R, joint->offset );

    if ( joint->node[1].body )
    {
        dVector3 anchor2;

        // get the anchor2 in global coordinates
        dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );

        q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
            ( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
        q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
            ( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
        q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
            ( joint->node[1].body->posr.pos[2] + anchor2[2] ) );

    }
    else
    {
        //N.B. When there is no body 2 the joint->anchor2 is already in
        //     global coordinates

        q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
            ( joint->anchor2[0] ) );
        q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
            ( joint->anchor2[1] ) );
        q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
            ( joint->anchor2[2] ) );

        if ( joint->flags & dJOINT_REVERSE )
        {
            q[0] = -q[0];
            q[1] = -q[1];
            q[2] = -q[2];
        }
    }

    dVector3 axP;
    // get prismatic axis in global coordinates
    dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 );

    return dCalcVectorDot3( axP, q );
}

void getAxis( dxJoint *j, dVector3 result, dVector3 axis1 )
{
    if ( j->node[0].body )
    {
        dMultiply0_331( result, j->node[0].body->posr.R, axis1 );
    }
}

void dMassRotate (dMass *m, const dMatrix3 R)
{
  // if the body is rotated by `R' relative to its point of reference,
  // the new inertia about the point of reference is:
  //
  //   R * I * R'
  //
  // where I is the old inertia.

  dMatrix3 t1;
  dReal t2[3];

  dAASSERT (m);

  // rotate inertia matrix
  dMultiply2_333 (t1,m->I,R);
  dMultiply0_333 (m->I,R,t1);

  // ensure perfect symmetry
  m->_I(1,0) = m->_I(0,1);
  m->_I(2,0) = m->_I(0,2);
  m->_I(2,1) = m->_I(1,2);

  // rotate center of mass
  dMultiply0_331 (t2,R,m->c);
  m->c[0] = t2[0];
  m->c[1] = t2[1];
  m->c[2] = t2[2];

# ifndef dNODEBUG
  dMassCheck (m);
# endif
}

void dxTriMesh::computeAABB() {
    const dxTriMeshData* d = Data;
    dVector3 c;
    const dMatrix3& R = final_posr->R;
    const dVector3& pos = final_posr->pos;
    
    dMultiply0_331( c, R, d->AABBCenter );
    
    dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) +
        dFabs(R[1] * Data->AABBExtents[1]) + 
        dFabs(R[2] * Data->AABBExtents[2]);
    dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) +
        dFabs(R[5] * Data->AABBExtents[1]) + 
        dFabs(R[6] * Data->AABBExtents[2]);
    dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) +
        dFabs(R[9] * Data->AABBExtents[1]) + 
        dFabs(R[10] * Data->AABBExtents[2]);

    aabb[0] = c[0] + pos[0] - xrange;
    aabb[1] = c[0] + pos[0] + xrange;
    aabb[2] = c[1] + pos[1] - yrange;
    aabb[3] = c[1] + pos[1] + yrange;
    aabb[4] = c[2] + pos[2] - zrange;
    aabb[5] = c[2] + pos[2] + zrange;
}

void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z)
{
    dAASSERT (g);
    dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
    CHECK_NOT_LOCKED (g->parent_space);
    if (g->offset_posr) {
        // move body such that body+offset = position
        dVector3 world_offset;
        dMultiply0_331(world_offset, g->body->posr.R, g->offset_posr->pos);
        dBodySetPosition(g->body,
            x - world_offset[0],
            y - world_offset[1],
            z - world_offset[2]);
    }
    else if (g->body) {
        // this will call dGeomMoved (g), so we don't have to
        dBodySetPosition (g->body,x,y,z);
    }
    else {
        g->final_posr->pos[0] = x;
        g->final_posr->pos[1] = y;
        g->final_posr->pos[2] = z;
        dGeomMoved (g);
    }
}

void
dxJointUniversal::getAxes( dVector3 ax1, dVector3 ax2 )
{
    // This says "ax1 = joint->node[0].body->posr.R * joint->axis1"
    dMultiply0_331( ax1, node[0].body->posr.R, axis1 );

    if ( node[1].body )
    {
        dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
    }
    else
    {
        ax2[0] = axis2[0];
        ax2[1] = axis2[1];
        ax2[2] = axis2[2];
    }
}

void dJointAddHinge2Torques( dJointID j, dReal torque1, dReal torque2 )
{
    dxJointHinge2* joint = ( dxJointHinge2* )j;
    dVector3 axis1, axis2;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, Hinge2 );

    if ( joint->node[0].body && joint->node[1].body )
    {
        dMultiply0_331( axis1, joint->node[0].body->posr.R, joint->axis1 );
        dMultiply0_331( axis2, joint->node[1].body->posr.R, joint->axis2 );
        axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
        axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
        axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
        dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] );
        dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] );
    }
}

void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 )
{
    if ( j->node[0].body )
    {
        dMultiply0_331( result, j->node[0].body->posr.R, anchor1 );
        result[0] += j->node[0].body->posr.pos[0];
        result[1] += j->node[0].body->posr.pos[1];
        result[2] += j->node[0].body->posr.pos[2];
    }
}

void setBall( dxJoint *joint, dxJoint::Info2 *info,
              dVector3 anchor1, dVector3 anchor2 )
{
    // anchor points in global coordinates with respect to body PORs.
    dVector3 a1, a2;

    int s = info->rowskip;

    // set jacobian
    info->J1l[0] = 1;
    info->J1l[s+1] = 1;
    info->J1l[2*s+2] = 1;
    dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
    dSetCrossMatrixMinus( info->J1a, a1, s );
    if ( joint->node[1].body )
    {
        info->J2l[0] = -1;
        info->J2l[s+1] = -1;
        info->J2l[2*s+2] = -1;
        dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
        dSetCrossMatrixPlus( info->J2a, a2, s );
    }

    // set right hand side
    dReal k = info->fps * info->erp;
    if ( joint->node[1].body )
    {
        for ( int j = 0; j < 3; j++ )
        {
            info->c[j] = k * ( a2[j] + joint->node[1].body->posr.pos[j] -
                               a1[j] - joint->node[0].body->posr.pos[j] );
        }
    }
    else
    {
        for ( int j = 0; j < 3; j++ )
        {
            info->c[j] = k * ( anchor2[j] - a1[j] -
                               joint->node[0].body->posr.pos[j] );
        }
    }
}

void setFixedOrientation( dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row )
{
    int s = info->rowskip;
    int start_index = start_row * s;

    // 3 rows to make body rotations equal
    info->J1a[start_index] = 1;
    info->J1a[start_index + s + 1] = 1;
    info->J1a[start_index + s*2+2] = 1;
    if ( joint->node[1].body )
    {
        info->J2a[start_index] = -1;
        info->J2a[start_index + s+1] = -1;
        info->J2a[start_index + s*2+2] = -1;
    }

    // compute the right hand side. the first three elements will result in
    // relative angular velocity of the two bodies - this is set to bring them
    // back into alignment. the correcting angular velocity is
    //   |angular_velocity| = angle/time = erp*theta / stepsize
    //                      = (erp*fps) * theta
    //    angular_velocity  = |angular_velocity| * u
    //                      = (erp*fps) * theta * u
    // where rotation along unit length axis u by theta brings body 2's frame
    // to qrel with respect to body 1's frame. using a small angle approximation
    // for sin(), this gives
    //    angular_velocity  = (erp*fps) * 2 * v
    // where the quaternion of the relative rotation between the two bodies is
    //    q = [cos(theta/2) sin(theta/2)*u] = [s v]

    // get qerr = relative rotation (rotation error) between two bodies
    dQuaternion qerr, e;
    if ( joint->node[1].body )
    {
        dQuaternion qq;
        dQMultiply1( qq, joint->node[0].body->q, joint->node[1].body->q );
        dQMultiply2( qerr, qq, qrel );
    }
    else
    {
        dQMultiply3( qerr, joint->node[0].body->q, qrel );
    }
    if ( qerr[0] < 0 )
    {
        qerr[1] = -qerr[1];  // adjust sign of qerr to make theta small
        qerr[2] = -qerr[2];
        qerr[3] = -qerr[3];
    }
    dMultiply0_331( e, joint->node[0].body->posr.R, qerr + 1 );  // @@@ bad SIMD padding!
    dReal k = info->fps * info->erp;
    info->c[start_row] = 2 * k * e[0];
    info->c[start_row+1] = 2 * k * e[1];
    info->c[start_row+2] = 2 * k * e[2];
}

void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
  if (!g) return;
  if (!pos) pos = dGeomGetPosition (g);
  if (!R) R = dGeomGetRotation (g);

  int type = dGeomGetClass (g);
  if (type == dBoxClass) {
    dVector3 sides;
    dGeomBoxGetLengths (g,sides);
    dsDrawBox (pos,R,sides);
  }
  else if (type == dSphereClass) {
    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
  }
  else if (type == dCapsuleClass) {
    dReal radius,length;
    dGeomCapsuleGetParams (g,&radius,&length);
    dsDrawCapsule (pos,R,length,radius);
  }
  else if (type == dCylinderClass) {
    dReal radius,length;
    dGeomCylinderGetParams (g,&radius,&length);
    dsDrawCylinder (pos,R,length,radius);
  }

  else if (type == dGeomTransformClass) {
    dGeomID g2 = dGeomTransformGetGeom (g);
    const dReal *pos2 = dGeomGetPosition (g2);
    const dReal *R2 = dGeomGetRotation (g2);
    dVector3 actual_pos;
    dMatrix3 actual_R;
    dMultiply0_331 (actual_pos,R,pos2);
    actual_pos[0] += pos[0];
    actual_pos[1] += pos[1];
    actual_pos[2] += pos[2];
    dMultiply0_333 (actual_R,R,R2);
    drawGeom (g2,actual_pos,actual_R,0);
  }

  if (show_aabb) {
    // draw the bounding box for this geom
    dReal aabb[6];
    dGeomGetAABB (g,aabb);
    dVector3 bbpos;
    for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
    dVector3 bbsides;
    for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
    dMatrix3 RI;
    dRSetIdentity (RI);
    dsSetColorAlpha (1,0,0,0.5);
    dsDrawBox (bbpos,RI,bbsides);
  }
}

/*******************************************************************************
Function to draw a geometry object.
*******************************************************************************/
void GOdeObject::drawGeom( dGeomID g, const dReal *position, const dReal *orientation ) const
{
	if( !g )		//If the geometry object is missing, end the function.
		return;

	if( !position )	//Position was not passed?
		position = dGeomGetPosition( g );		//Then, get the geometry position.

	if( !orientation )	//Orientation was not given?
		orientation = dGeomGetRotation( g );	//And get existing geometry orientation.

	int type = dGeomGetClass( g );				//Get the type of geometry.
	
	if( type == dBoxClass )						//Is it a box?
	{
		dReal sides[3];
		dGeomBoxGetLengths( g, sides );				//Get length of sides.
		renderBox( sides, position, orientation );	//Render the actual box in environment.
	}

	if( type == dSphereClass )					//Is it a sphere?
	{
		dReal radius;
		radius = dGeomSphereGetRadius( g );				//Get the radius.
		renderSphere( radius, position, orientation );	//Render sphere in environment.
	}

	if( type == dCapsuleClass )
	{
		dReal radius;
		dReal length;
		dGeomCapsuleGetParams( g, &radius, &length );	//Get both radius and length.
		renderCapsule( radius, length, position, orientation );	//Render capsule in environment.
	}

	if( type == dGeomTransformClass )					//Is it an embeded geom in a composite body.
	{
		dGeomID g2 = dGeomTransformGetGeom( g );		//Get the actual geometry inside the wrapper.
		const dReal *position2 = dGeomGetPosition( g2 );	//Get position and orientation of wrapped geometry.
		const dReal *orientation2 = dGeomGetRotation( g2 );
		
		dVector3 actualPosition;						//Real world coordinated position and orientation
		dMatrix3 actualOrientation;						//of the wrapped geometry.
		
		dMultiply0_331( actualPosition, orientation, position2 );	//Get world coordinates of geometry position.
		actualPosition[0] += position[0];
		actualPosition[1] += position[1];
		actualPosition[2] += position[2];

		dMultiply0_333( actualOrientation, orientation, orientation2 );	//Get world coordinates of geom orientation.

		drawGeom( g2, actualPosition, actualOrientation );	//Draw embeded geometry.
	}
}

void dJointGetHinge2Axis2( dJointID j, dVector3 result )
{
    dxJointHinge2* joint = ( dxJointHinge2* )j;
    dUASSERT( joint, "bad joint argument" );
    dUASSERT( result, "bad result argument" );
    checktype( joint, Hinge2 );
    if ( joint->node[1].body )
    {
        dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis2 );
    }
}

dReal dJointGetScrewPosition ( dJointID j )
{
    dxJointScrew* joint = ( dxJointScrew* ) j;
    dUASSERT ( joint, "bad joint argument" );
    checktype ( joint, Screw );

    // get axis1 in global coordinates
    dVector3 ax1, q;
    if (!joint->node[0].body)
      return 0;

    dMultiply0_331 ( ax1, joint->node[0].body->posr.R, joint->axis1 );

    if (joint->node[1].body)
    {
        // get body2 + offset point in global coordinates
        dMultiply0_331 ( q, joint->node[1].body->posr.R, joint->offset );
        for (int i = 0; i < 3; ++i )
            q[i] = joint->node[0].body->posr.pos[i]
                   - q[i]
                   - joint->node[1].body->posr.pos[i];
    }
    else
    {
        q[0] = joint->node[0].body->posr.pos[0] - joint->offset[0];
        q[1] = joint->node[0].body->posr.pos[1] - joint->offset[1];
        q[2] = joint->node[0].body->posr.pos[2] - joint->offset[2];

        if ( joint->flags & dJOINT_REVERSE )
        {
            // N.B. it could have been simplier to only inverse the sign of
            //      the dCalcVectorDot3 result but this case is exceptional and
            //      doing the check for all case can decrease the performance.
            ax1[0] = -ax1[0];
            ax1[1] = -ax1[1];
            ax1[2] = -ax1[2];
        }
    }

    return dCalcVectorDot3 ( ax1, q );
}