C++ NxVec3::magnitude方法代码示例

unsigned int Forest::onCollideJumper(unsigned int id, Matrix4f* matrix, void* data)
{
    Forest* __this = reinterpret_cast<Forest*>( data );

    // determine obb of instance
    NxBox instanceOBB = calculateOBB( 
        __this->_canopyBatch->getBatchScheme()->lodGeometry[0], 
        *matrix,
        __this->_desc.collScale
    );
    __this->_debugBoxes.push_back( instanceOBB );

    // collide obbs
    if( NxBoxBoxIntersect( instanceOBB, __this->_jumperOBB ) )
    {
        Jumper* jumper = dynamic_cast<Jumper*>( __this->_currentJumper );
        // add impulse to jumper body
        NxVec3 linearVelocity = __this->_currentJumperActor->getLinearVelocity();
        NxVec3 impulse = linearVelocity * getCore()->getRandToolkit()->getUniform( __this->_desc.minImpulseFactor, __this->_desc.maxImpulseFactor ) * -1;
        NxVec3 localPos(
            __this->_jumperOBB.extents.x * getCore()->getRandToolkit()->getUniform( -1, 1 ),
            __this->_jumperOBB.extents.y * getCore()->getRandToolkit()->getUniform( -1, 1 ),
            __this->_jumperOBB.extents.z * getCore()->getRandToolkit()->getUniform( -1, 1 )
        );
        __this->_currentJumperActor->addForceAtLocalPos( impulse, localPos, NX_IMPULSE );
        // damage jumper
        jumper->damage( __this->_desc.damageFactor * impulse.magnitude(), 0.0f, linearVelocity.magnitude() );
        // play rustle sound
        __this->playRustleSound( __this->_currentJumperCollision->getFrame()->getPos() );
    }

    return id;
}

NxDistanceJoint* World::CreateDistanceJoint(NxActor* a0, NxActor* a1, const NxVec3& anchor0, const NxVec3& anchor1, const NxVec3& globalAxis)
{
	NxDistanceJointDesc distanceDesc;
	distanceDesc.actor[0] = a0;
	distanceDesc.actor[1] = a1;
	distanceDesc.localAnchor[0] = anchor0;
	distanceDesc.localAnchor[1] = anchor1;
	distanceDesc.setGlobalAxis(globalAxis);

	NxVec3 dist = a1->getGlobalPose()*anchor1 - a0->getGlobalPose()*anchor0;
	printf("dist is %f %f %f\n",dist.x, dist.y, dist.z);
	distanceDesc.maxDistance = dist.magnitude()*1.5f;
	distanceDesc.minDistance = dist.magnitude()*0.1f;

	printf("maxDistance %f, minDistance %f\n", distanceDesc.maxDistance, distanceDesc.minDistance);

	NxSpringDesc spring;
	spring.spring = 1000;		// х»лн¤х╩§
	spring.damper = 0.5;
	distanceDesc.spring = spring;
	distanceDesc.flags = (NX_DJF_MIN_DISTANCE_ENABLED | NX_DJF_MAX_DISTANCE_ENABLED);
	distanceDesc.flags |= NX_DJF_SPRING_ENABLED;
	distanceDesc.jointFlags |= NX_JF_COLLISION_ENABLED;

	return (NxDistanceJoint*)gScene->createJoint(distanceDesc);
}

unsigned int Forest::onCollideCanopy(unsigned int id, Matrix4f* matrix, void* data)
{
    Forest* __this = reinterpret_cast<Forest*>( data );

    // determine obb of instance
    NxBox instanceOBB = calculateOBB( 
        __this->_canopyBatch->getBatchScheme()->lodGeometry[0], 
        *matrix,
        __this->_desc.collScale
    );
    __this->_debugBoxes.push_back( instanceOBB );

    // collide obbs
    if( NxBoxBoxIntersect( instanceOBB, __this->_canopyOBB ) )
    {
        CanopySimulator* canopy = dynamic_cast<CanopySimulator*>( __this->_currentCanopy );
        // calculate intersection details
        float volume;
        NxVec3 globalIntersectionCenter;
        calculateIntersectionDetails( instanceOBB, __this->_canopyOBB, volume, globalIntersectionCenter );
        assert( volume <= 1.0f );
        // add force to canopy
        NxVec3 linearVelocity = __this->_currentCanopyActor->getLinearVelocity();
        float linearVelocityMagnitude = linearVelocity.magnitude();
        linearVelocity.normalize();
        NxVec3 force = linearVelocity * 0.2f * sqr( linearVelocityMagnitude ) * __this->_currentCanopyInfo->square * -volume;
        __this->_currentCanopyActor->addForceAtPos( force, globalIntersectionCenter, NX_FORCE );
        // damage canopy
        canopy->rip( __this->_desc.ripFactor * force.magnitude() );
        // entangle canopy
        if( volume > __this->_desc.entangleFactor )
        {            
            canopy->entangle( globalIntersectionCenter );
            __this->playSqueakSound( wrap( globalIntersectionCenter ) );
        }
        else
        {
            __this->playRustleSound( wrap( globalIntersectionCenter ) );
        }
    }

    return id;
}

void DrawForce(NxActor* actor, NxVec3& forceVec, const NxVec3& color)
{
	// Draw only if the force is large enough
	NxReal force = forceVec.magnitude();
	if (force < 0.1f)  return;

	forceVec = 3*forceVec/force;

	NxVec3 pos = actor->getCMassGlobalPosition();
	DrawArrow(pos, pos + forceVec, color);
}

void Jumper::CanopyOpening::updatePhysics(void)
{
    // velocity of base jumper's body
    NxVec3 velocity = _phActor->getLinearVelocity();

    // local coordinate system of base jumper
    NxMat34 pose = _phActor->getGlobalPose();
    NxVec3 x = pose.M.getColumn(0);
    NxVec3 y = pose.M.getColumn(1);
    NxVec3 z = pose.M.getColumn(2);

    // air resistance force
    float AR = _jumper->getVirtues()->getTrackingAirResistance();

    // terminal velocity
    float Vt = sqrt( 9.8f * _phActor->getMass() / AR );
    float It = velocity.magnitude() / Vt;

    // air resistance force
    NxVec3 Far = NxVec3(0,1,0) * getAirResistancePower( velocity.magnitude() / Vt ) * _phActor->getMass() * 9.8f;

    // finalize motion equation    
    _phActor->addForce( Far );

    // linear damping is function of jumper velocity    
    // this is prevents calculation errors due to high speed rates
    float minVel     = 50.0f;
    float minDamping = _initialLD;
    float maxVel     = 70.0f;
    float maxDamping = 2.5f;
    float factor = ( velocity.magnitude() - minVel ) / ( maxVel - minVel );
    factor = factor < 0 ? 0 : ( factor > 1 ? 1 : factor );
    float damping = minDamping * ( factor - 1 ) + maxDamping * ( factor );
    _phActor->setLinearDamping( damping );

    // shallow brake setting
    _canopy->setLeftDeep( 0.0f );
    _canopy->setRightDeep( 0.0f );

}

    void DynamicImage::update()
    {
        


        if (!sleep)
        {
            NxVec3 towardsVector = tarPos - curPos;

            switch (mode)
            {
            case 0:     //Linear
                if (towardsVector.magnitude() < LINEAR_VELOCITY)
                {
                    curPos = tarPos;
                    sleep = true;
                }
                else
                {
                    towardsVector.normalize();
                    curPos = curPos + (towardsVector * LINEAR_VELOCITY);
                }
                break;


            case 1:     //Halfling
                if (towardsVector.magnitude() < THRESHOLD)
                {
                    curPos = tarPos;
                    sleep = true;
                }
                else
                {
                    curPos = curPos + (towardsVector * 0.033f);
                }
                break;

            }
        }
    }

void DrawForceAtShape(NxActor* actor, NxShape* shape, NxVec3& forceVec, const NxVec3& color)
{
	// Draw only if the force is large enough
	NxReal force = forceVec.magnitude();
	if (force < 0.1f)  return;

	forceVec = 3*forceVec/force;

	NxVec3 pos;	
	if (bShapeSelectMode && bForceMode)
		pos = shape->getGlobalPosition();
	else
		pos = actor->getCMassGlobalPosition();
	DrawArrow(pos, pos + forceVec, color);
}

void CreateExperiment()
{
	switch (gExperimentType)
	{
	case LANDER:
		  {
			  gScene->releaseActor(*groundPlane); groundPlane=NULL; // will be recreated with the container cell
			  CreateExperimentMaterials();
			  CreateContainerCell(gCellSize);
			  gPhysicsSDK->setParameter(NX_VISUALIZE_ACTOR_AXES,0);
			  gPhysicsSDK->setParameter(NX_VISUALIZE_COLLISION_FNORMALS,0);
			  gPhysicsSDK->setParameter(NX_VISUALIZE_COLLISION_SHAPES,1);
			  gPhysicsSDK->setParameter(NX_VISUALIZE_WORLD_AXES,0);
			  //gLander=CreateLander(NxVec3(0.0f,0.0f,0.0f));
			  gLander=CreateLander(NxVec3(0.0f,gCellSize/2.0f,0.0f));
			  IdentifyGravitators();
			  if (gLander) {
			  ReOrientActor(gLander);
			  SpinActor(gLander,gLandingRoughness*sqrt(gCellSize*gDefaultGravity.magnitude()/(gLander->getMassSpaceInertiaTensor().magnitude())));
			  LaunchActor(gLander,gLandingRoughness*sqrt(gCellSize*gDefaultGravity.magnitude()));
			  gSelectedActor=gLander;
			  gPhysicsSDK->setParameter(NX_VISUALIZE_ACTOR_AXES,gLanderSize.magnitude());
			  isRunning=true;
			  }
			  else {
				  printf("Error: Unable to create lander\a\n");
				  isRunning=false;
			  }
			  break;
		  }
	case LAY_SUBSTRATE:
		{
			gScene->releaseActor(*groundPlane); groundPlane=NULL; // will be recreated with the container cell
			CreateExperimentMaterials();
			CreateContainerCell(gCellSize);
			gPhysicsSDK->setParameter(NX_VISUALIZE_ACTOR_AXES,0);
			gPhysicsSDK->setParameter(NX_VISUALIZE_COLLISION_FNORMALS,0);
			gPhysicsSDK->setParameter(NX_VISUALIZE_COLLISION_SHAPES,1);
			gPhysicsSDK->setParameter(NX_VISUALIZE_WORLD_AXES,0);
			//ThrowStone();
			isRunning=true;
			break;
		}
	default:				
		{printf("Error: unimplemented experiment type\a\n"); isRunning=false; break;}
	}
}

void PxSingleActor::sweepTest( MatrixF *mat )
{
   NxVec3 nxCurrPos = getPosition();

   // If the position is zero, 
   // the parent hasn't been updated yet
   // and we don't even need to do the sweep test.
   // This is a fix for a problem that was happening
   // where on the add of the PxSingleActor, it would
   // set the position to a very small value because it would be getting a hit
   // even though the current position was 0.
   if ( nxCurrPos.isZero() )
      return;
   
   // Set up the flags and the query structure.
   NxU32 flags = NX_SF_STATICS | NX_SF_DYNAMICS;

   NxSweepQueryHit sweepResult;
   dMemset( &sweepResult, 0, sizeof( sweepResult ) );

   NxVec3 nxNewPos = mat->getPosition();

   // Get the velocity which will be our sweep direction and distance.
   NxVec3 nxDir = nxNewPos - nxCurrPos;
   if ( nxDir.isZero() )
      return;

   // Get the scene and do the sweep.
   mActor->wakeUp();
   mActor->linearSweep( nxDir, flags, NULL, 1, &sweepResult, NULL );

   if ( sweepResult.hitShape && sweepResult.t < nxDir.magnitude() )
   {
      nxDir.normalize();
      nxDir *= sweepResult.t;
      nxCurrPos += nxDir;

      mat->setPosition( Point3F( nxCurrPos.x, nxCurrPos.y, nxCurrPos.z ) );
   }
}

void PxSingleActor::applyCorrection( const MatrixF& mat, const NxVec3& linVel, const NxVec3& angVel )
{
   // Sometimes the actor hasn't been
   // created yet during the call from unpackUpdate.
   if ( !mActor || !mWorld )
      return;

   mWorld->releaseWriteLock();

   NxVec3 newPos = mat.getPosition();
   NxVec3 currPos = getPosition();

   NxVec3 offset = newPos - currPos;

   // If the difference isn't large enough,
   // just set the new transform, no correction.
   if ( offset.magnitude() > 0.3f )
   {
      // If we're going to set the linear or angular velocity,
      // we do it before we add a corrective force, since it would be overwritten otherwise.
      NxVec3 currLinVel, currAngVel;
      currLinVel = mActor->getLinearVelocity();
      currAngVel = mActor->getAngularVelocity();

      // Scale the corrective force by half,
      // otherwise it will over correct and oscillate.
      NxVec3 massCent = mActor->getCMassGlobalPosition();
      mActor->addForceAtPos( offset, massCent, NX_VELOCITY_CHANGE );

      // If the linear velocity is divergent enough, change to server linear velocity.
      if ( (linVel - currLinVel).magnitude() > 0.3f )
         mActor->setLinearVelocity( linVel );
      // Same for angular.
      if ( (angVel - currAngVel).magnitude() > 0.3f )
         mActor->setAngularVelocity( angVel );   
   }

   Parent::setTransform( mat );
}

void WindPointer::onEvent(Actor* initiator, unsigned int eventId, void* eventData)
{
    if( eventId == EVENT_CAMERA_IS_ACTUAL )
    {
        // current wind
        NxVec3 wind = _scene->getWindAtPoint( NxVec3( 0,0,0 ) );

        // make signature worldspace position
        Matrix4f cameraPose = _scene->getCamera()->getPose();
        Vector3f cameraPos( cameraPose[3][0], cameraPose[3][1], cameraPose[3][2] );
        Vector3f pos( wind.x, wind.y, wind.z );
        pos.normalize();
        pos *= 10000.0f;
        pos += cameraPos;

        // update signature window
        Vector3f screenPos = Gameplay::iEngine->getDefaultCamera()->projectPosition( pos );
        if( screenPos[2] > 1 )
        {
            _signature->getPanel()->setVisible( false );
            return;
        }
        else
        {
            _signature->getPanel()->setVisible( true );
            gui::Rect oldRect = _signature->getPanel()->getRect();
            gui::Rect newRect(
                int( screenPos[0] ), int( screenPos[1] ),
                int( screenPos[0] ) + oldRect.getWidth(),
                int( screenPos[1] ) + oldRect.getHeight()
            );
            _signature->getPanel()->setRect( newRect );
            _windSpeed->getStaticText()->setText( wstrformat( Gameplay::iLanguage->getUnicodeString(295), wind.magnitude() ).c_str() );
        }        
    }
}

// This is the generic sweep test for all swept volumes, but not character-controller specific
bool SweepTest::DoSweepTest(void* user_data,
							void* user_data2,
							NxU32 nb_boxes, const NxExtendedBounds3* boxes, const void** box_user_data,
							NxU32 nb_capsules, const NxExtendedCapsule* capsules, const void** capsule_user_data,
							SweptVolume& swept_volume,
							const NxVec3& direction, NxU32 max_iter, NxU32* nb_collisions,
							NxU32 group_flags, float min_dist, const NxGroupsMask* groupsMask, bool down_pass)
{
	// Early exit when motion is zero. Since the motion is decomposed into several vectors
	// and this function is called for each of them, it actually happens quite often.
	if(direction.isZero())
		return false;

	bool HasMoved = false;
	mValidTri = false;

	NxExtendedVec3 CurrentPosition = swept_volume.mCenter;
	NxExtendedVec3 TargetPosition = swept_volume.mCenter;
	TargetPosition += direction;

	NxU32 NbCollisions = 0;
	while(max_iter--)
	{
		gNbIters++;
		// Compute current direction
		NxVec3 CurrentDirection = TargetPosition - CurrentPosition;

		// Make sure the new TBV is still valid
		{
			// Compute temporal bounding box. We could use a capsule or an OBB instead:
			// - the volume would be smaller
			// - but the query would be slower
			// Overall it's unclear whether it's worth it or not.
			// TODO: optimize this part ?
			NxExtendedBounds3 TemporalBox;
			swept_volume.ComputeTemporalBox(*this, TemporalBox, CurrentPosition, CurrentDirection);

			// Gather touched geoms
			UpdateTouchedGeoms(user_data, swept_volume,
								nb_boxes, boxes, box_user_data,
								nb_capsules, capsules, capsule_user_data,
								group_flags, TemporalBox, groupsMask);
		}

		const float Length = CurrentDirection.magnitude();
		if(Length<min_dist)	break;

		CurrentDirection /= Length;

		// From Quake2: "if velocity is against the original velocity, stop dead to avoid tiny occilations in sloping corners"
		if((CurrentDirection|direction) <= 0.0f)	break;

		// From this point, we're going to update the position at least once
		HasMoved = true;

		// Find closest collision
		SweptContact C;
		C.mDistance = Length + mSkinWidth;

		if(!CollideGeoms(this, swept_volume, mGeomStream, CurrentPosition, CurrentDirection, C))
		{
			// no collision found => move to desired position
			CurrentPosition = TargetPosition;
			break;
		}

		ASSERT(C.mGeom);	// If we reach this point, we must have touched a geom

		if(C.mGeom->mType==TOUCHED_USER_BOX || C.mGeom->mType==TOUCHED_USER_CAPSULE)
		{
			// We touched a user object, typically another CCT
			if(mValidateCallback)	UserHitCallback(user_data2, C, CurrentDirection, Length);

			// Trying to solve the following problem:
			// - by default, the CCT "friction" is infinite, i.e. a CCT will not slide on a slope (this is by design)
			// - this produces bad results when a capsule CCT stands on top of another capsule CCT, without sliding. Visually it looks
			//   like the character is standing on the other character's head, it looks bad. So, here, we would like to let the CCT
			//   slide away, i.e. we don't want friction.
			// So here we simply increase the number of iterations (== let the CCT slide) when the first down collision is with another CCT.
			if(down_pass && !NbCollisions)
				max_iter += 9;
		}
		else
		{
			// We touched a normal object
#ifdef USE_CONTACT_NORMAL_FOR_SLOPE_TEST
			mValidTri = true;
			mCN = C.mWorldNormal;
#else
			if(C.mIndex!=INVALID_ID)
			{
				mValidTri = true;
				mTouched = mWorldTriangles[C.mIndex];
			}
#endif
			{
				if(mValidateCallback)	ShapeHitCallback(user_data2, C, CurrentDirection, Length);
			}
		}
//.........这里部分代码省略.........

//.........这里部分代码省略.........
		totalInputLeft = toggleLeft;
	} else if (spinalCord->leftRearRiser > toggleLeft && spinalCord->leftRearRiser > spinalCord->leftWarp) {
		totalInputLeft = spinalCord->leftRearRiser;
	} else {
		totalInputLeft = spinalCord->leftWarp;
	}
	if (toggleRight > spinalCord->rightRearRiser && toggleRight > spinalCord->rightWarp) {
		totalInputRight = toggleRight;
	} else if (spinalCord->rightRearRiser > toggleRight && spinalCord->rightRearRiser > spinalCord->rightWarp) {
		totalInputRight = spinalCord->rightRearRiser;
	} else {
		totalInputRight = spinalCord->rightWarp;
	}


	if( totalInputLeft != 0 && totalInputRight == 0 ) {
		if (canopy->getLeftStowed() && spinalCord->trigger_left) {
			_targetSequence = &unstowLeftSequence;
		} else if (spinalCord->modifier) {
			_targetSequence = &steerLeftSequenceHalf;
		} else {
			_targetSequence = &steerLeftSequence;
		}
	}
	if( totalInputRight != 0 && totalInputLeft == 0 ) {
		if (canopy->getRightStowed() && spinalCord->trigger_right) {
			_targetSequence = &unstowRightSequence;
		} else if (spinalCord->modifier) {
			_targetSequence = &steerRightSequenceHalf;
		} else {
			_targetSequence = &steerRightSequence;
		}
	}
	if( totalInputLeft != 0 && totalInputRight != 0 ) {
		if (canopy->getLeftStowed() || canopy->getRightStowed()) {
			_targetSequence = &groupingUnstowSequence;
		} else if (spinalCord->modifier) {
			_targetSequence = &groupingSequenceHalf;
		} else {
			_targetSequence = &groupingSequence;
		}
	}

	// unstow toggles
	if (spinalCord->left > 0.1f) {
		canopy->setLeftStowed(false);
	}
	if (spinalCord->right > 0.1f) {
		canopy->setRightStowed(false);
	}

    // local coordinate system of base jumper
    NxMat34 pose = _phActor->getGlobalPose();
    NxVec3 x = pose.M.getColumn(0);
    NxVec3 y = pose.M.getColumn(1);
    NxVec3 z = pose.M.getColumn(2);



	// altitude [m]
	//NxVec3 pos = _phActor->getGlobalPosition();
	//const float altitude = pos.y;
	//const float arch_pose_area = 1.9f;

	//// air density: converted to linear from barometric equation [0:10] km altitude
	//// http://www.denysschen.com/catalogue/density.aspx
	//const float AirDensityOld = altitude <= 10000.0f ? (1.196f - 0.0000826f * altitude) : (0.27f);

	//// add drag force
	//float Cd = 0.4f;	// altitude suit = 0.4f
	//const float mTracking = database::Suit::getRecord( _jumper->getVirtues()->equipment.suit.id )->mTracking;
	//Cd *= mTracking;
	//// Drag force vector
	//NxVec3 VecFd = -_phActor->getLinearVelocity();
	//VecFd.normalize();

	//float Fd = 0.5f * AirDensityOld * velocity.magnitudeSquared() * Cd * arch_pose_area * getCore()->getRandToolkit()->getUniform(0.94f, 1.06f);
	//_phActor->addForceAtLocalPos(Fd*VecFd, NxVec3(0, -3.0f, 0));

    // air resistance coefficient
    const float ARmult = 2.5f;
    float AR = ARmult * virtues->getTrackingAirResistance();
    if( database::Suit::getRecord( virtues->equipment.suit.id )->wingsuit )
    {
        AR = ARmult * virtues->getFrogAirResistance();
    }

    // terminal velocity
    const float Vt = sqrt( 9.8f * _phActor->getMass() / AR );
    const float It = velocity.magnitude() / Vt;

    NxVec3 dir = velocity * -1;
    dir.normalize();

    // air resistance force
    const NxVec3 Far = dir * getAirResistancePower( velocity.magnitude() / Vt ) * _phActor->getMass() * 9.8f;

    // finalize motion equation    
    _phActor->addForce( Far );
}

void CanopySimulator::Rope::initialize(NxActor* actor1, NxVec3 anchor1, NxActor* actor2, NxVec3 anchor2)
{
    _nxActor1 = actor1; assert( _nxActor1 );
    _nxActor2 = actor2; assert( _nxActor2 );
    _anchor1 = anchor1;
    _anchor2 = anchor2;

    // calculate worldspace positions for connections points
    Matrix4f ltm1  = wrap( _nxActor1->getGlobalPose() );
    Matrix4f ltm2  = wrap( _nxActor2->getGlobalPose() );
    NxVec3 pos1 = wrap( Gameplay::iEngine->transformCoord( wrap( _anchor1 ), ltm1 ) );
    NxVec3 pos2 = wrap( Gameplay::iEngine->transformCoord( wrap( _anchor2 ), ltm2 ) );
    NxVec3 dir  = pos2 - pos1;

    // check distance btw connection points (correction just for stability purpose)
    float idist = dir.magnitude();
    dir.normalize();

    // create intermediate segment bodies
    NxVec3 pos = pos1;
    float step = idist / _numJoints;
    for( unsigned int i=0; i<_numJoints-1; i++ )
    {
        // segment position
        pos += dir * step;
        // create segment body
        NxBodyDesc nxBodyDesc;
        nxBodyDesc.mass = _mass / ( _numJoints - 1 );
        nxBodyDesc.massSpaceInertia.set( nxBodyDesc.mass, nxBodyDesc.mass, nxBodyDesc.mass );
        nxBodyDesc.linearDamping = 2.0f;
        nxBodyDesc.angularDamping = 0.0f;
        nxBodyDesc.flags = NX_BF_VISUALIZATION;
        nxBodyDesc.solverIterationCount = 32;
        NxActorDesc nxActorDesc;
        nxActorDesc.body = &nxBodyDesc;
        nxActorDesc.globalPose.M.id();
        nxActorDesc.globalPose.t = pos;
        _nxSegmentBody[i] = _nxScene->createActor( nxActorDesc );
        assert( _nxSegmentBody[i] );
        // initialize velocity
        _nxSegmentBody[i]->addForce( _nxActor1->getLinearVelocity(), NX_VELOCITY_CHANGE );
    }

    // create joints
    for( i=0; i<_numJoints; i++ )
    {
        // first joint (actor1-segment1)
        if( i == 0 )
        {
            NxDistanceJointDesc jointDesc;
            jointDesc.actor[0] = _nxActor1;
            jointDesc.actor[1] = _nxSegmentBody[0];
            jointDesc.maxDistance = _length / _numJoints;
            jointDesc.minDistance = 0.0f;
            jointDesc.flags = NX_DJF_MAX_DISTANCE_ENABLED;
            jointDesc.localAnchor[0] = _anchor1;
            jointDesc.localAnchor[1].set( 0,0,0 );
            jointDesc.jointFlags = NX_JF_VISUALIZATION;
            _nxSegmentJoint[i] = _nxScene->createJoint( jointDesc );
            assert( _nxSegmentJoint[i] );
        }
        // last joint (segmentN-actor2)
        else if( i == _numJoints-1 )
        {
            NxDistanceJointDesc jointDesc;
            jointDesc.actor[0] = _nxSegmentBody[_numJoints-2];
            jointDesc.actor[1] = _nxActor2;
            jointDesc.maxDistance = _length / _numJoints;
            jointDesc.minDistance = 0.0f;
            jointDesc.flags = NX_DJF_MAX_DISTANCE_ENABLED;
            jointDesc.localAnchor[0].set( 0,0,0 );
            jointDesc.localAnchor[1] = _anchor2;
            jointDesc.jointFlags = NX_JF_VISUALIZATION;
            _nxSegmentJoint[i] = _nxScene->createJoint( jointDesc );
            assert( _nxSegmentJoint[i] );
        }
        // intermediate joint
        else
        {
            NxDistanceJointDesc jointDesc;
            jointDesc.actor[0] = _nxSegmentBody[i-1];
            jointDesc.actor[1] = _nxSegmentBody[i];
            jointDesc.maxDistance = _length / _numJoints;
            jointDesc.minDistance = 0.0f;
            jointDesc.flags = NX_DJF_MAX_DISTANCE_ENABLED;
            jointDesc.localAnchor[0].set( 0,0,0 );
            jointDesc.localAnchor[1].set( 0,0,0 );
            jointDesc.jointFlags = NX_JF_VISUALIZATION;
            _nxSegmentJoint[i] = _nxScene->createJoint( jointDesc );
            assert( _nxSegmentJoint[i] );
        }
    }
}

void DrawWireCapsule(const NxCapsule& capsule, const NxVec3& color)
{
	NxReal r = capsule.radius;
	NxVec3 dir = capsule.p0 - capsule.p1;
	NxReal h = dir.magnitude();
	NxMat34 pose;
	pose.t = (capsule.p0 + capsule.p1)*0.5f;

	if(h!=0.0f)
	{
		dir/=h;
		NxVec3 right, up;
		computeBasis(dir, right, up);
		pose.M.setColumn(0, right);
		pose.M.setColumn(1, dir);
		pose.M.setColumn(2, up);
	}
	else
	{
		pose.M.id();
	}

//	NxMat34 pose = capsule->getGlobalPose();
//	const NxReal & r = capsule->isCapsule()->getRadius();
//	const NxReal & h = capsule->isCapsule()->getHeight();



	NxSegment SG;
	pose.M.getColumn(1, SG.p1);
	SG.p1 *= 0.5f*h;
	SG.p0 = -SG.p1;
	SG.p0 += pose.t;
	SG.p1 += pose.t;

	NxVec3 c0;	pose.M.getColumn(0, c0);
	NxVec3 c1;	pose.M.getColumn(1, c1);
	NxVec3 c2;	pose.M.getColumn(2, c2);
	DrawLine(SG.p0 + c0*r, SG.p1 + c0*r, color);
	DrawLine(SG.p0 - c0*r, SG.p1 - c0*r, color);
	DrawLine(SG.p0 + c2*r, SG.p1 + c2*r, color);
	DrawLine(SG.p0 - c2*r, SG.p1 - c2*r, color);

	pose.M.setColumn(0, -c1);
	pose.M.setColumn(1, -c0);
	pose.M.setColumn(2, c2);
	pose.t = SG.p0;
	DrawCircle(20, pose, color, r, true);	//halfcircle -- flipped

	pose.M.setColumn(0, c1);
	pose.M.setColumn(1, -c0);
	pose.M.setColumn(2, c2);
	pose.t = SG.p1;
	DrawCircle(20, pose, color, r, true);

	pose.M.setColumn(0, -c1);
	pose.M.setColumn(1, c2);
	pose.M.setColumn(2, c0);
	pose.t = SG.p0;
	DrawCircle(20, pose, color, r, true);//halfcircle -- good

	pose.M.setColumn(0, c1);
	pose.M.setColumn(1, c2);
	pose.M.setColumn(2, c0);
	pose.t = SG.p1;
	DrawCircle(20, pose, color, r, true);

	pose.M.setColumn(0, c2);
	pose.M.setColumn(1, c0);
	pose.M.setColumn(2, c1);
	pose.t = SG.p0;
	DrawCircle(20, pose, color, r);	//full circle
	pose.t = SG.p1;
	DrawCircle(20, pose, color, r);
}