// ray-sphere intersection test from Graphics Gems p.388
// **NOTE** There is a bug in this Graphics Gem.  If the origin
// of the ray is *inside* the sphere being tested, it reports the
// wrong intersection location.  This code has a fix for the bug.
bool  SBM_rayIntersectsSphere(const float *point,const float *direction,const float *_center,float radius,float &t)
{
  bool ret = false;

  NxVec3 rayOrigin(point);
  NxVec3 dir(direction);
  NxVec3 center(_center);

	// notation:
	// point E  = rayOrigin
	// point O  = sphere center

	NxVec3 EO = center - rayOrigin;
  NxVec3 V = dir;

  float dist2 = EO.x*EO.x + EO.y*EO.y + EO.z * EO.z;

  float r2 = radius*radius;

  // Bug Fix For Gem, if origin is *inside* the sphere, invert the
  // direction vector so that we get a valid intersection location.
  if ( dist2 < r2 )
    V*=-1;

	float v = EO.dot(V);

	float disc = r2 - (EO.magnitudeSquared() - v*v);

	if (disc > 0.0f)
	{
    t = sqrtf(disc);
		ret = true;
	}

	return ret;
}
Example #2
0
void Controller::move(SweptVolume& volume, const NxVec3& disp, NxU32 activeGroups, NxF32 minDist, NxU32& collisionFlags, NxF32 sharpness, const NxGroupsMask* groupsMask, bool constrainedClimbingMode)
	{
	// Dynamic-load the utility library
	if(!gUtilLib)
		gUtilLib = NxGetUtilLib();
	assert(gUtilLib);
	if(!gUtilLib)	return;

	SweepTest* ST = &cctModule;

	// Init CCT with per-controller settings
	ST->debugData		= manager->debugData;
	ST->mSkinWidth		= skinWidth;
	ST->mStepOffset		= stepOffset;
	ST->mUpDirection	= upDirection;
	ST->mHandleSlope	= handleSlope;
	ST->mSlopeLimit		= slopeLimit;
	ST->mFirstUpdate	= true;

	///////////

	Controller** boxUserData = NULL;
	NxExtendedBounds3* boxes = NULL;
	NxU32 nbBoxes = 0;

	Controller** capsuleUserData = NULL;
	NxExtendedCapsule* capsules = NULL;
	NxU32 nbCapsules = 0;

	if(1)
		{
		// Experiment - to do better
		NxU32 nbControllers = manager->getNbControllers();
		Controller** controllers = manager->getControllers();

		boxes = (NxExtendedBounds3*)NxAlloca(nbControllers*sizeof(NxExtendedBounds3));
		capsules = (NxExtendedCapsule*)NxAlloca(nbControllers*sizeof(NxExtendedCapsule));	// It's evil to waste that ram
		boxUserData = (Controller**)NxAlloca(nbControllers*sizeof(Controller*));
		capsuleUserData = (Controller**)NxAlloca(nbControllers*sizeof(Controller*));

		while(nbControllers--)
			{
			Controller* currentController = *controllers++;
			if(currentController==this)	continue;

			NxActor* pActor = currentController->getActor();
			int nbShapes = pActor->getNbShapes();
			NX_ASSERT( nbShapes == 1 );
			NxShape* pCurrentShape= pActor->getShapes()[0];

			// Depending on user settings the current controller can be:
			// - discarded
			// - always kept
			// - or tested against filtering flags
			NxCCTInteractionFlag interactionFlag = currentController->getInteraction();
			bool keepController = true;
			if(interactionFlag==NXIF_INTERACTION_EXCLUDE)			keepController = false;
			else if(interactionFlag==NXIF_INTERACTION_USE_FILTER)	keepController = (activeGroups & ( 1 << pCurrentShape->getGroup()))!=0;

			if(keepController)
				{
				if(currentController->type==NX_CONTROLLER_BOX)
					{
					currentController->getWorldBox(boxes[nbBoxes]);
					boxUserData[nbBoxes++] = currentController;
					}
				else if(currentController->type==NX_CONTROLLER_CAPSULE)
					{
					CapsuleController* CC = static_cast<CapsuleController*>(currentController);
					NxExtendedVec3 p0 = CC->position;
					NxExtendedVec3 p1 = CC->position;
					p0[ST->mUpDirection] -= CC->height*0.5f;
					p1[ST->mUpDirection] += CC->height*0.5f;
					capsules[nbCapsules].p0 = p0;
					capsules[nbCapsules].p1 = p1;
					capsules[nbCapsules].radius = CC->radius;
					capsuleUserData[nbCapsules++] = currentController;
					}
				else ASSERT(0);
				}
			}
		}

	///////////

	ST->mWalkExperiment = false;

	NxExtendedVec3 Backup = volume.mCenter;
	ST->MoveCharacter(scene,
		(Controller*)this,
		volume, disp,
		nbBoxes, nbBoxes ? boxes : NULL, nbBoxes ? (const void**)boxUserData : NULL,
		nbCapsules, nbCapsules ? capsules : NULL, nbCapsules ? (const void**)capsuleUserData : NULL,
		activeGroups, minDist, collisionFlags, groupsMask, constrainedClimbingMode);

	if(ST->mHitNonWalkable)
		{
		// A bit slow, but everything else I tried was less convincing...
		ST->mWalkExperiment = true;
		volume.mCenter = Backup;
		ST->MoveCharacter(scene,
			(Controller*)this,
			volume, disp,
			nbBoxes, nbBoxes ? boxes : NULL, nbBoxes ? (const void**)boxUserData : NULL,
			nbCapsules, nbCapsules ? capsules : NULL, nbCapsules ? (const void**)capsuleUserData : NULL,
			activeGroups, minDist, collisionFlags, groupsMask, constrainedClimbingMode);
		ST->mWalkExperiment = false;
		}

if(sharpness<0.0f)
volume.mCenter = Backup;

	// Copy results back
	position = volume.mCenter;

	NxVec3 Delta = Backup - volume.mCenter;
	NxF32 deltaM2 = Delta.magnitudeSquared();
	if(deltaM2!=0.0f)
		{
		// Update kinematic actor
		if(kineActor)
			kineActor->moveGlobalPosition(NxVec3((float)position.x, (float)position.y, (float)position.z));
		}

	filteredPosition = position;

sharpness = fabsf(sharpness);

	// Apply feedback filter if needed
	if(sharpness<1.0f)
		filteredPosition[upDirection] = feedbackFilter(position[upDirection], memory, sharpness);

//	if(manager->debugData)
//		manager->debugData->addAABB(cctModule.mCachedTBV, NX_ARGB_YELLOW);
	}
Example #3
0
void NxVehicle::updateVehicle(NxReal lastTimeStepSize)
{
	//printf("updating %x\n", this);
	
	NxReal distanceSteeringAxisCarTurnAxis = _steeringSteerPoint.x  - _steeringTurnPoint.x;
	NX_ASSERT(_steeringSteerPoint.z == _steeringTurnPoint.z);
	NxReal distance2 = 0;
	if (NxMath::abs(_steeringWheelState) > 0.01f)
		distance2 = distanceSteeringAxisCarTurnAxis / NxMath::tan(_steeringWheelState * _steeringMaxAngleRad);

	//printf("d1 = %2.3f, d2 = %2.3f, a1 = %2.3f, a2 = %2.3f\n",
	//	distanceSteeringAxisCarTurnAxis, distance2,
	//	_steeringWheelState, _steeringWheelState * _steeringMaxAngleRad);


	_lastTrailTime += lastTimeStepSize;

	if(_lastTrailTime > TRAIL_FREQUENCY)
	{
		_lastTrailTime = 0.0f;
	}

	NxU32 nbTouching = 0;
	NxU32 nbNotTouching = 0;
	NxU32 nbHandBrake = 0;
	for(NxU32 i = 0; i < _wheels.size(); i++)
	{
		NxWheel* wheel = _wheels[i];

		if (_lastTrailTime  == 0.0f)
		{
			if(_wheels[i]->hasGroundContact())
			{
				if (++_nextTrailSlot >= NUM_TRAIL_POINTS)
					_nextTrailSlot = 0;
				_trailBuffer[_nextTrailSlot] = _bodyActor->getGlobalPose() * _wheels[i]->getGroundContactPos();
			}
		}

		if(wheel->getWheelFlag(NX_WF_STEERABLE_INPUT))
		{
			if(distance2 != 0)
			{
				NxReal xPos = wheel->getWheelPos().x;
				NxReal zPos = wheel->getWheelPos().z;
				NxReal dz = -zPos + distance2;
				NxReal dx = xPos - _steeringTurnPoint.x;
				wheel->setAngle(NxMath::atan(dx/dz));
			} else {
				wheel->setAngle(0.f);
			}
			//printf("%2.3f\n", wheel->getAngle());

		} else if(wheel->getWheelFlag(NX_WF_STEERABLE_AUTO))
			{
			NxVec3 localVelocity = _bodyActor->getLocalPointVelocity(wheel->getWheelPos());
			NxQuat local2Global = _bodyActor->getGlobalOrientationQuat();
			local2Global.inverseRotate(localVelocity);
//			printf("%2.3f %2.3f %2.3f\n", wheel->getWheelPos().x,wheel->getWheelPos().y,wheel->getWheelPos().z);
			localVelocity.y = 0;
			if(localVelocity.magnitudeSquared() < 0.01f)
			{
				wheel->setAngle(0.0f);
			} else {
				localVelocity.normalize();
//				printf("localVelocity: %2.3f %2.3f\n", localVelocity.x, localVelocity.z);
				if(localVelocity.x < 0)
					localVelocity = -localVelocity;
				NxReal angle = NxMath::clamp((NxReal)atan(localVelocity.z / localVelocity.x), 0.3f, -0.3f);
				wheel->setAngle(angle);
			}
		}

		// now the acceleration part
		if(!wheel->getWheelFlag(NX_WF_ACCELERATED))
			continue;

		if(_handBrake && wheel->getWheelFlag(NX_WF_AFFECTED_BY_HANDBRAKE))
		{
			nbHandBrake++;
		} else {
			if (!wheel->hasGroundContact())
			{
				nbNotTouching++;
			} else {
				nbTouching++;
			}
		}
	}
	
	NxReal motorTorque = 0.0f; 
	if(nbTouching && NxMath::abs(_accelerationPedal) > 0.01f) 
	{
		NxReal axisTorque = _computeAxisTorque();
		NxReal wheelTorque = axisTorque / (NxReal)(_wheels.size() - nbHandBrake);
		NxReal wheelTorqueNotTouching = nbNotTouching>0?wheelTorque*(NxMath::pow(0.5f, (NxReal)nbNotTouching)):0;
		NxReal wheelTorqueTouching = wheelTorque - wheelTorqueNotTouching;
		motorTorque = wheelTorqueTouching / (NxReal)nbTouching; 
	} else {
		_updateRpms();
	}
//printf("wt: %f %f\n", motorTorque, _brakePedal);
	for(NxU32 i = 0; i < _wheels.size(); i++) 
	{
		NxWheel* wheel = _wheels[i];
		wheel->tick(_handBrake, motorTorque, _brakePedal, lastTimeStepSize);
	}

	//printf("---\n");
}
Example #4
0
void Airplane::onUpdatePhysics()
{
    if (_phActor != NULL) {
        if (_phActor->isSleeping()) {
            _phActor->wakeUp();
            return;
        }

        /// PHYSICS
        // altitude [m]
        NxVec3 pos = _phActor->getGlobalPosition();
        NxVec3 velocity = _phActor->getLinearVelocity();
        float altitude = pos.y;

        // speed (m/s) squared
        const float Vsq = velocity.magnitudeSquared();

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

        // AOA
        NxMat34 globalPose = _phActor->getGlobalPose();
        NxVec3 x = globalPose.M.getColumn(0);
        NxVec3 y = globalPose.M.getColumn(1);
        NxVec3 z = globalPose.M.getColumn(2);

        NxVec3 velocityN = velocity;
        velocityN.normalize();
        const float AOA = -::calcAngle( z, velocityN, x );

        // ADD DRAG
        const float Cd = 0.027f;
        const float Fd = 0.5f * AirDensityOld * Vsq * Cd * 10.0f;

        // crosssectional objects
        const unsigned int xsections_c = 1;
        NxVec3 xsections[xsections_c];
        // fuselage
        //_phActor->addLocalForce(wrap(_propellerFrame->getAt()) * 100.0f);

        // synchronize physics & render
        //_clump->getFrame()->setPos(wrap(_phActor->getGlobalPosition()));
        //_clump->getFrame()->setMatrix(wrap(_phActor->getGlobalPose()));
    }

    if( !_roughMode )
    {
        float dt = ::simulationStepTime;

        _clump->getAnimationController()->advance( dt * _desc.animationSpeed );
        _clump->getFrame()->getLTM();

        // landing mode
        if( _landingMode && _clump->getFrame()->getPos()[1] > _desc.lastAltitude )
        {
            _clump->getFrame()->translate( Vector3f( 0.0f, -_desc.loweringSpeed * dt, 0.0f ) );
        }

        // calculate velocity
        Vector3f currPoint = _propellerFrame->getPos();
        _velocity = ( currPoint - _prevPoint ) / ( dt );
        _prevPoint = currPoint;
    }

    // fly by waypoints
    if( _desc.waypoints.size() && _waypointId < _desc.waypoints.size() - 1 )
    {
        // current waypoints
        AirplaneDesc::WayPoint waypoint1 = _desc.waypoints[_waypointId];
        AirplaneDesc::WayPoint waypoint2 = _desc.waypoints[_waypointId+1];

        // trajectory vector
        Vector3f trajectory = waypoint2.pos - waypoint1.pos;

        // current velocity
        float vel = waypoint1.vel * ( 1 - _waypointFactor ) + waypoint2.vel * _waypointFactor;

        // motion distance
        float distance = vel * ::simulationStepTime;

        // determine factor distance
        float fDistance = distance / trajectory.length();

        // move in factor space
        _waypointFactor += fDistance;

        // out of current waypoints?
        if( _waypointFactor > 1 )
        {
            // determine rest of distance
            fDistance = _waypointFactor - 1;
            distance = fDistance * trajectory.length();
            _waypointFactor = 0;
            _waypointId++;

            if( _waypointId < _desc.waypoints.size() - 1 )
            {
                // new waypoints
                waypoint1 = _desc.waypoints[_waypointId];
                waypoint2 = _desc.waypoints[_waypointId+1];

                // new trajectory vector
                trajectory = waypoint2.pos - waypoint1.pos;

                // new factor distance
                fDistance = distance / trajectory.length();

                // move in factor space
                _waypointFactor += fDistance;
            }
            else
            {
                _waypointFactor = 0;
            }
        }
    }
}