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