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