//assumes that the Vectors are normalized hsBool ThreePlaneIntersect(const NxVec3& norm0, const NxVec3& point0, const NxVec3& norm1, const NxVec3& point1, const NxVec3& norm2, const NxVec3& point2, NxVec3& loc) { //need to make sure these planes aren't parallel hsBool suc=0; NxVec3 cross=norm1.cross( norm2); float denom=norm0.dot(cross); if(abs(denom)<0.0001) return 0;//basically paralell // if we are here there must be a point in 3 space try{ float d1,d2,d3; d1=norm0.dot(point0); d2=norm1.dot(point1); d3=norm2.dot(point2); NxVec3 n1Xn2=norm1.cross(norm2); NxVec3 n2Xn0=norm2.cross(norm0); NxVec3 n0Xn1=norm0.cross(norm1); NxVec3 pos=(d1*n1Xn2+ d2*n2Xn0 + d3*n0Xn1)/(denom); loc.x=pos.x; loc.y=pos.y; loc.z=pos.z; suc= 1; } catch(...) { suc=0; } return suc; }
// ----------------------------------------------------------------------------------- bool ObjMesh::rayTriangleIntersection(const NxVec3 &orig, const NxVec3 &dir, const ObjMeshTriangle &triangle, NxReal &t, NxReal &u, NxReal &v) const { const NxVec3 &a = mVertices[triangle.vertexNr[0]]; const NxVec3 &b = mVertices[triangle.vertexNr[0]]; const NxVec3 &c = mVertices[triangle.vertexNr[0]]; NxVec3 edge1, edge2, tvec, pvec, qvec; NxReal det,inv_det; edge1 = b - a; edge2 = c - a; pvec.cross(dir, edge2); /* if determinant is near zero, ray lies in plane of triangle */ det = edge1.dot(pvec); if (det == 0.0f) return false; inv_det = 1.0f / det; /* calculate distance from vert0 to ray origin */ tvec = orig - a; /* calculate U parameter and test bounds */ u = tvec.dot(pvec) * inv_det; if (u < 0.0f || u > 1.0f) return false; /* prepare to test V parameter */ qvec.cross(tvec, edge1); /* calculate V parameter and test bounds */ v = dir.dot(qvec) * inv_det; if (v < 0.0f || u + v > 1.0f) return false; /* calculate t, ray intersects triangle */ t = edge2.dot(qvec) * inv_det; return true; }
// 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; }
Jumper::CanopyOpening::CanopyOpening(Jumper* jumper, NxActor* phFreeFall, NxActor* phFlight, MatrixConversion* mcFlight, PilotchuteSimulator* pc, CanopySimulator* c, NxVec3 fla, NxVec3 fra, NxVec3 rla, NxVec3 rra) : JumperAction( jumper ) { // set action properties _actionTime = 0.0f; _blendTime = 0.2f; _endOfAction = false; _phActor = phFlight; _matrixConversion = mcFlight; _pilotchute = pc; _canopy = c; _frontLeftAnchor = fla; _frontRightAnchor = fra; _rearLeftAnchor = rla; _rearRightAnchor = rra; _initialLD = phFlight->getLinearDamping(); // activate jumper body simulator Matrix4f sampleLTM = Jumper::getCollisionFC( _clump )->getFrame()->getLTM(); phFlight->setGlobalPose( wrap( sampleLTM ) ); phFlight->wakeUp(); phFlight->setLinearVelocity( phFreeFall->getLinearVelocity() ); phFlight->setAngularVelocity( phFreeFall->getAngularVelocity() ); // connect & open canopy _canopy->connect( _phActor, _frontLeftAnchor, _frontRightAnchor, _rearLeftAnchor, _rearRightAnchor, Jumper::getFrontLeftRiser( _clump ), Jumper::getFrontRightRiser( _clump ), Jumper::getRearLeftRiser( _clump ), Jumper::getRearRightRiser( _clump ) ); // age if (_jumper->getCanopyReserveSimulator() && _jumper->getCanopyReserveSimulator() == _canopy) { ++jumper->getVirtues()->equipment.reserve.age; } else { ++jumper->getVirtues()->equipment.canopy.age; } // retrieve pilotchute velocity float pcVel = pc->getPhActor()->getLinearVelocity().magnitude(); float anglVel = phFreeFall->getAngularVelocity().magnitude(); // retrieve pilotchute reference velocity database::Canopy* canopyInfo = _canopy->getGearRecord(); database::Pilotchute* pcInfo; if (_jumper->getCanopyReserveSimulator() == _canopy) { pcInfo = canopyInfo->pilots; } else { assert( canopyInfo->numPilots > _jumper->getVirtues()->equipment.pilotchute ); pcInfo = canopyInfo->pilots + _jumper->getVirtues()->equipment.pilotchute; } // probability of lineover float lineoverProb = _jumper->getVirtues()->getLineoverProbability( pcVel / pcInfo->Vrec ); // no slider has not smaller probability if (_jumper->getVirtues()->equipment.sliderOption != soRemoved) { lineoverProb *= 0.3f; } // reserves have 70% less lineover probability if (_jumper->getCanopyReserveSimulator() == _canopy) { lineoverProb *= 0.3f; } bool leftLineover = ( getCore()->getRandToolkit()->getUniform( 0, 1 ) < lineoverProb ); bool rightLineover = !leftLineover && ( getCore()->getRandToolkit()->getUniform( 0, 1 ) < lineoverProb ); float leftLOW = leftLineover ? getCore()->getRandToolkit()->getUniform( 0.5, 1.0f ) : 0; float rightLOW = rightLineover ? getCore()->getRandToolkit()->getUniform( 0.5, 1.0f ) : 0; // probability of linetwists // add probability if spinning //if( _jumper->isPlayer() ) getCore()->logMessage( "angular velocity component on deployment: %2.10f", anglVel * 0.13f ); float linetwistsProb = _jumper->getVirtues()->getLinetwistsProbability( pcVel ) + anglVel * 0.13f; float linetwistsDice = getCore()->getRandToolkit()->getUniform( 0.0f, 1.0f ); // probability of linetwists because of incorrect body position NxVec3 motionDir = _jumper->getFreefallActor()->getLinearVelocity(); motionDir.normalize(); NxVec3 canopyDown = _jumper->getFreefallActor()->getGlobalPose().M.getColumn(2); canopyDown.normalize(); float relativity = 1.0f + fabs(canopyDown.dot( motionDir )); linetwistsProb *= relativity; // reserves have 50% less linetwist probability if (_jumper->getCanopyReserveSimulator() == _canopy) { linetwistsProb *= 0.5f; } // base canopies gave 40% less linetwist probability else if (!canopyInfo->skydiving) { linetwistsProb *= 0.4f; } bool linetwists = ( linetwistsDice <= linetwistsProb ); //if( _jumper->isPlayer() ) { //getCore()->logMessage( "linetwists prob: %3.5f", linetwistsProb ); //getCore()->logMessage( "linetwists dice: %3.5f", linetwistsDice ); //} else { //getCore()->logMessage( "linetwists prob BOT: %3.5f", linetwistsProb ); //getCore()->logMessage( "linetwists dice BOT: %3.5f", linetwistsDice ); //} // generate linetwists (positive is righttwist, negative is lefttwist) // the smaller the canopy, the heavier the linetwist float sq = canopyInfo->square * 2.0f; // 300 canopy: 120; 840 // 200 canopy: 320; 1040 // 100 canopy: 520; 1240 float linetwistsAngle = getCore()->getRandToolkit()->getUniform( 720 - sq, 1540 - sq ); if (linetwistsAngle > 1080.0f) { linetwistsAngle = 1440.0f; } else if (linetwistsAngle < 320.0f) { linetwistsAngle = 180.0f; } float sign = getCore()->getRandToolkit()->getUniform( -1, 1 ); sign = sign < 0.0f ? -1.0f : ( sign > 0.0f ? 1.0f : 0.0f ); linetwistsAngle *= sign; if( !linetwists ) linetwistsAngle = 0.0f; // test, force linetwist //if (!_jumper->isPlayer() && _jumper->getCanopyReserveSimulator() != _canopy) { // linetwists = true; // linetwistsAngle = 1440.0f; //} // offheading : canopy turns by specified angle float minTurn = 30.0f; // rigging skill = 0.0 float maxTurn = 10.0f; // rigging skill = 1.0 float rigging = _jumper->getVirtues()->getRiggingSkill(); assert( rigging >= 0 && rigging <= 1 ); float turn = minTurn * ( 1 - rigging ) + maxTurn * rigging; float angle = getCore()->getRandToolkit()->getUniform( -turn, turn ); //if( _jumper->isPlayer() ) getCore()->logMessage( "additional turn (offheading): %2.1f", angle ); //if( !_jumper->isPlayer() ) angle = 0; Vector3f sampleX( sampleLTM[0][0], sampleLTM[0][1], sampleLTM[0][2] ); Vector3f sampleY( sampleLTM[1][0], sampleLTM[1][1], sampleLTM[1][2] ); Vector3f sampleZ( sampleLTM[2][0], sampleLTM[2][1], sampleLTM[2][2] ); Vector3f sampleP( sampleLTM[3][0], sampleLTM[3][1], sampleLTM[3][2] ); // orient canopy towards jumper velocity sampleZ = wrap( phFlight->getLinearVelocity() ); sampleZ *= -1; sampleZ.normalize(); sampleY = _jumper->getClump()->getFrame()->getAt() * -1; sampleX.cross( sampleY, sampleZ ); sampleX.normalize(); sampleY.cross( sampleZ, sampleX ); sampleY.normalize(); sampleLTM.set( sampleX[0], sampleX[1], sampleX[2], 0.0f, sampleY[0], sampleY[1], sampleY[2], 0.0f, sampleZ[0], sampleZ[1], sampleZ[2], 0.0f, 0.0f, 0.0f, 0.0f, 1.0f ); // turn canopy by random angle sampleLTM = Gameplay::iEngine->rotateMatrix( sampleLTM, sampleZ, angle ); // move clump behind jumper sampleP -= sampleZ * 80.0f; sampleLTM[3][0] = sampleP[0]; sampleLTM[3][1] = sampleP[1]; sampleLTM[3][2] = sampleP[2]; _canopy->open( wrap( sampleLTM ), _phActor->getLinearVelocity(), leftLOW, rightLOW, linetwistsAngle ); // reconnect pilotchute to canopy _canopy->getClump()->getFrame()->getLTM(); _pilotchute->connect( _canopy->getNxActor(), CanopySimulator::getPilotCordJoint( _canopy->getClump() ), _canopy->getPilotAnchor() ); //_pilotchute->setFreebag(_canopy == _jumper->getCanopyReserveSimulator()); set in jumper constructor and jumper::fireReserveCanopy() // put to sleep freefall simulator phFreeFall->putToSleep(); phFreeFall->raiseActorFlag( NX_AF_DISABLE_COLLISION ); // show risers Jumper::getRisers( _clump )->setFlags( engine::afRender ); // animation controller engine::IAnimationController* animCtrl = _clump->getAnimationController(); // capture blend source animCtrl->captureBlendSrc(); // reset animation mixer for( unsigned int i=0; i<engine::maxAnimationTracks; i++ ) { if( animCtrl->getTrackAnimation( i ) ) animCtrl->setTrackActivity( i, false ); } // setup animation animCtrl->setTrackAnimation( 0, &openingSequence ); animCtrl->setTrackActivity( 0, true ); animCtrl->setTrackSpeed( 0, 0.75f ); animCtrl->setTrackWeight( 0, 1.0f ); animCtrl->resetTrackTime( 0 ); animCtrl->advance( 0.0f ); // capture blend destination animCtrl->captureBlendDst(); animCtrl->blend( 0.0f ); }
hsVectorStream* plPhysXCooking::IMakePolytope(const plMaxMeshExtractor::NeutralMesh& inMesh) { hsBool success=0; std::vector<hsPoint3> outCloud; hsPoint3 offset; int numPlanes=26; float planeMax[26]; int indexMax[26]; hsPoint3 AABBMin(FLT_MAX,FLT_MAX,FLT_MAX); hsPoint3 AABBMax(-FLT_MAX,-FLT_MAX,-FLT_MAX); //prep NxVec3* vectors = new NxVec3[26]; int curvec=0; for(int xcomp= -1;xcomp<2;xcomp++) { for(int ycomp= -1;ycomp<2;ycomp++) { for(int zcomp= -1;zcomp<2;zcomp++) { if(!((xcomp==0)&&(ycomp==0)&&(zcomp==0))) { vectors[curvec].set((float)(xcomp),(float)(ycomp),(float)(zcomp)); vectors[curvec].normalize(); planeMax[curvec]=(-FLT_MAX); //indexMax[curvec]=0; curvec++; } } } } /* for(int i=0;i<26;i++) {//make your max and mins planeMax[i]=(-FLT_MAX); } */ hsPoint3 centroid(0.0f,0.0f,0.0f); for(int i=0;i<inMesh.fNumVerts;i++) centroid+=inMesh.fVerts[i]; centroid=centroid/(float)inMesh.fNumVerts; //temp NxVec3* nxLocs=new NxVec3[inMesh.fNumVerts]; NxVec3* nxLocs2=new NxVec3[inMesh.fNumVerts]; for(int i=0;i<inMesh.fNumVerts;i++) { hsPoint3 temppt=inMesh.fVerts[i] - centroid; nxLocs[i]=plPXConvert::Point(temppt); } NxMat33 rot; NxVec3 eigen; PCA(nxLocs,inMesh.fNumVerts,rot); NxMat33 invrot; rot.getInverse(invrot); for(int i=0; i<inMesh.fNumVerts;i++) { nxLocs2[i]=invrot*nxLocs[i]; } for(int i=0;i<inMesh.fNumVerts;i++) { for(int plane=0;plane<26;plane++) { float dist=nxLocs2[i].dot(vectors[plane]); if(dist>=planeMax[plane]) { planeMax[plane]=dist; indexMax[plane]=i; } } } for(int i=0;i<inMesh.fNumVerts;i++) { AABBMin.fX = hsMinimum(nxLocs2[i].x, AABBMin.fX); AABBMin.fY = hsMinimum(nxLocs2[i].y, AABBMin.fY); AABBMin.fZ = hsMinimum(nxLocs2[i].z, AABBMin.fZ); AABBMax.fX = hsMaximum(nxLocs2[i].x, AABBMax.fX); AABBMax.fY = hsMaximum(nxLocs2[i].y, AABBMax.fY); AABBMax.fZ = hsMaximum(nxLocs2[i].z, AABBMax.fZ); } int resultingPoints=0; for(int i=0;i<26;i++) { for(int j=0;j<26;j++) { for(int k=0;k<26;k++) { NxVec3 res; if(ThreePlaneIntersect(vectors[i],nxLocs2[indexMax[i]],vectors[j],nxLocs2[indexMax[j]], vectors[k],nxLocs2[indexMax[k]],res)) { //check it is within all slabs bool within=true; int curplane=0; do { float intersecdist=res.dot(vectors[curplane]); if((intersecdist-planeMax[curplane])>0.0001) { within=false; } curplane++; } while((curplane<26)&&within); if(within) // if((res.x>=AABBMin.fX)&&(res.x<=AABBMax.fX)&& // (res.y>=AABBMin.fY)&&(res.y<=AABBMax.fY)&& // (res.z>=AABBMin.fZ)&&(res.z<=AABBMax.fZ)) { NxVec3 reverted; reverted=rot*res; reverted.x=reverted.x +centroid.fX; reverted.y=reverted.y +centroid.fY; reverted.z=reverted.z +centroid.fZ; hsPoint3 out; out=plPXConvert::Point(reverted); outCloud.push_back(out); } } } } } //planes discovered //this is'nt right //cleanup offset=centroid; delete[] vectors; hsPoint3* pointages=new hsPoint3[outCloud.size()]; for(int x=0;x<outCloud.size();x++)pointages[x]=outCloud[x]; hsVectorStream* vectorstrm; vectorstrm= CookHull(outCloud.size(),pointages,true); delete[] pointages; delete[] nxLocs; delete[] nxLocs2; return vectorstrm; }
void pWheel2::_updateVirtoolsEntity(bool position,bool rotation) { CK3dEntity *ent = static_cast<CK3dEntity*>(GetPMan()->GetContext()->GetObject(getEntID())); if (ent && position) { NxWheelShape *wShape = getWheelShape(); NxMat34 pose = wShape->getGlobalPose(); NxWheelContactData wcd; NxShape* contactShape = wShape->getContact(wcd); NxVec3 suspensionOffsetDirection; pose.M.getColumn(1, suspensionOffsetDirection); suspensionOffsetDirection =-suspensionOffsetDirection; if (contactShape && wcd.contactForce > -1000) { NxVec3 toContact = wcd.contactPoint - pose.t; double alongLength = suspensionOffsetDirection.dot(toContact); NxVec3 across = toContact - alongLength * suspensionOffsetDirection; double r = wShape->getRadius(); double pullBack = sqrt(r*r - across.dot(across)); pose.t += (alongLength - pullBack) * suspensionOffsetDirection; } else { pose.t += wShape->getSuspensionTravel() * suspensionOffsetDirection; } VxVector oPos = getFrom(pose.t); ent->SetPosition(&oPos); if (hasGroundContact()) { NxWheelShape *wShape = getWheelShape(); NxMat34& wheelPose = wShape->getGlobalPose(); /* NxWheelContactData wcd; NxShape* cShape = wShape->getContact(wcd); NxReal stravel = wShape->getSuspensionTravel(); NxReal radius = wShape->getRadius(); VxVector gPos = getFrom(getWheelPose().t); /* if( cShape && wcd.contactPosition <= (stravel + radius) ) { }*/ ////////////////////////////////////////////////////////////////////////// /*VxVector gPos = getFrom(getWheelPose().t); //gPos*=-1.0f; gPos -=getWheelPos(); V 3. xVector gPos2 = getFrom(getWheelShape()->getLocalPose().t); ent->SetPosition(&gPos2,getBody()->GetVT3DObject()); */ }else { // VxVector gPos = getWheelPos(); // ent->SetPosition(&gPos,getBody()->GetVT3DObject()); } } if (ent && rotation) { //float rollAngle = getWheelRollAngle(); //rollAngle+=wShape->getAxleSpeed() * (dt* 0.01f); VxQuaternion rot = pMath::getFrom( getWheelPose().M ); ent->SetQuaternion(&rot,NULL); } /* NxWheelShape *wShape = getWheelShape(); while (rollAngle > NxTwoPi) //normally just 1x rollAngle-= NxTwoPi; while (rollAngle< -NxTwoPi) //normally just 1x rollAngle+= NxTwoPi; setWheelRollAngle(rollAngle); NxMat34& wheelPose = wShape->getGlobalPose(); NxWheelContactData wcd; NxShape* cShape = wShape->getContact(wcd); NxReal stravel = wShape->getSuspensionTravel(); NxReal radius = wShape->getRadius(); //have ground contact? if( cShape && wcd.contactPosition <= (stravel + radius) ) { wheelPose.t = NxVec3( wheelPose.t.x, wcd.contactPoint.y + getRadius(), wheelPose.t.z ); } else { wheelPose.t = NxVec3( wheelPose.t.x, wheelPose.t.y - getSuspensionTravel(), wheelPose.t.z ); } float rAngle = rollAngle; float steer = wShape->getSteerAngle(); NxMat33 rot, axisRot, rollRot; rot.rotY( wShape->getSteerAngle() ); axisRot.rotY(0); rollRot.rotX(rollAngle); wheelPose.M = rot * wheelPose.M * axisRot * rollRot; setWheelPose(wheelPose); */ }