static PxTransform computeBoneTransform(PxTransform &rootTransform, Acclaim::Bone &bone, PxVec3* boneFrameData) { using namespace Acclaim; //PxTransform rootTransform(PxVec3(0.0f), PxQuat(PxIdentity)); PxTransform parentTransform = (bone.mParent) ? computeBoneTransform(rootTransform, *bone.mParent, boneFrameData) : rootTransform; PxQuat qWorld = EulerAngleToQuat(bone.mAxis); PxVec3 offset = bone.mLength * bone.mDirection; PxQuat qDelta = PxQuat(PxIdentity); PxVec3 boneData = boneFrameData[bone.mID-1]; if (bone.mDOF & BoneDOFFlag::eRX) qDelta = PxQuat(Ps::degToRad(boneData.x), PxVec3(1.0f, 0.0f, 0.0f)) * qDelta; if (bone.mDOF & BoneDOFFlag::eRY) qDelta = PxQuat(Ps::degToRad(boneData.y), PxVec3(0.0f, 1.0f, 0.0f)) * qDelta; if (bone.mDOF & BoneDOFFlag::eRZ) qDelta = PxQuat(Ps::degToRad(boneData.z), PxVec3(0.0f, 0.0f, 1.0f)) * qDelta; PxQuat boneOrientation = qWorld * qDelta * qWorld.getConjugate(); PxTransform boneTransform(boneOrientation.rotate(offset), boneOrientation); return parentTransform.transform(boneTransform); }
void Sc::BodySim::calculateKinematicVelocity(PxReal oneOverDt) { PX_ASSERT(isKinematic()); /*------------------------------------------------\ | kinematic bodies are moved directly by the user and are not influenced by external forces | we simply determine the distance moved since the last simulation frame and | assign the appropriate delta to the velocity. This vel will be used to shove dynamic | objects in the solver. | We have to do this like so in a delayed way, because when the user sets the target pos the dt is not | yet known. \------------------------------------------------*/ PX_ASSERT(isActive()); BodyCore& core = getBodyCore(); if (readInternalFlag(BF_KINEMATIC_MOVED)) { clearInternalFlag(BF_KINEMATIC_SETTLING); const SimStateData* kData = core.getSimStateData(true); PX_ASSERT(kData); PX_ASSERT(kData->isKine()); PX_ASSERT(kData->getKinematicData()->targetValid); PxVec3 linVelLL, angVelLL; PxTransform targetPose = kData->getKinematicData()->targetPose; const PxTransform& currBody2World = getBody2World(); //the kinematic target pose is now the target of the body (CoM) and not the actor. PxVec3 deltaPos = targetPose.p; deltaPos -= currBody2World.p; linVelLL = deltaPos * oneOverDt; PxQuat q = targetPose.q * currBody2World.q.getConjugate(); if (q.w < 0) //shortest angle. q = -q; PxReal angle; PxVec3 axis; q.toRadiansAndUnitAxis(angle, axis); angVelLL = axis * angle * oneOverDt; core.setLinearVelocity(linVelLL); core.setAngularVelocity(angVelLL); // Moving a kinematic should trigger a wakeUp call on a higher level. PX_ASSERT(core.getWakeCounter()>0); PX_ASSERT(isActive()); } else { core.setLinearVelocity(PxVec3(0)); core.setAngularVelocity(PxVec3(0)); } }
void SampleSubmarine::onSubstep(float dtime) { // user input -> forces handleInput(); // change current every 0.01s static PxReal sElapsedTime = 0.0f; sElapsedTime += mPause ? 0 : dtime; if(sElapsedTime > 0.01f) { static PxReal angle = 0; angle += sElapsedTime*0.01f; angle = angle < (PxTwoPi) ? angle : angle - PxTwoPi; sElapsedTime = 0; gBuoyancy.z = 0.15f * PxSin(angle * 50); PxQuat yRot = PxQuat(angle, PxVec3(0,1,0)); gBuoyancy = yRot.rotate(gBuoyancy); // apply external forces to seamines const size_t nbSeamines = mSeamines.size(); for(PxU32 i = 0; i < nbSeamines; i++) { Seamine* mine = mSeamines[i]; mine->mMineHead->addForce(gBuoyancy, PxForceMode::eACCELERATION); const size_t nbLinks = mine->mLinks.size(); for(PxU32 j = 0; j < nbLinks; j++) { mine->mLinks[j]->addForce(gBuoyancy, PxForceMode::eACCELERATION); } } } if(mSubmarineActor) { //convert forces from submarine the submarine's body local space to global space PxQuat submarineOrientation = mSubmarineActor->getGlobalPose().q; gForce = submarineOrientation.rotate(gForce); gTorque = submarineOrientation.rotate(gTorque); // add also current forces to submarine gForce.z += gBuoyancy.z * 5.0f; // apply forces in global space and reset mSubmarineActor->addForce(gForce); mSubmarineActor->addTorque(gTorque); gForce = PxVec3(0); gTorque = PxVec3(0); } }
static bool setMassAndUpdateInertia(bool multipleMassOrDensity, PxRigidBody& body, const PxReal* masses, PxU32 massCount, const PxVec3* massLocalPose, bool includeNonSimShapes) { bool success; // default values in case there were no shapes PxReal massOut = 1.0f; PxVec3 diagTensor(1.0f,1.0f,1.0f); PxQuat orient = PxQuat(PxIdentity); bool lockCom = massLocalPose != NULL; PxVec3 com = lockCom ? *massLocalPose : PxVec3(0); const char* errorStr = "PxRigidBodyExt::setMassAndUpdateInertia"; if(masses && massCount) { Ext::InertiaTensorComputer inertiaComp(true); if(computeMassAndInertia(multipleMassOrDensity, body, NULL, masses, massCount, includeNonSimShapes, inertiaComp)) { success = true; if (inertiaComp.getMass()!=0 && !computeMassAndDiagInertia(inertiaComp, diagTensor, orient, massOut, com, lockCom, body, errorStr)) success = false; // computeMassAndDiagInertia() failed (mass zero?) if (massCount == 1) massOut = masses[0]; // to cover special case where body has no simulation shape } else { Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "%s: Mass and inertia computation failed, setting mass to 1 and inertia to (1,1,1)", errorStr); success = false; } } else { Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "%s: No mass specified, setting mass to 1 and inertia to (1,1,1)", errorStr); success = false; } PX_ASSERT(orient.isFinite()); PX_ASSERT(diagTensor.isFinite()); body.setMass(massOut); body.setMassSpaceInertiaTensor(diagTensor); body.setCMassLocalPose(PxTransform(com, orient)); return success; }
void UPhysicsHandleComponent::UpdateHandleTransform(const FTransform& NewTransform) { if(!KinActorData) { return; } #if WITH_PHYSX bool bChangedPosition = true; bool bChangedRotation = true; PxRigidDynamic* KinActor = KinActorData; // Check if the new location is worthy of change PxVec3 PNewLocation = U2PVector(NewTransform.GetTranslation()); PxVec3 PCurrentLocation = KinActor->getGlobalPose().p; if((PNewLocation - PCurrentLocation).magnitudeSquared() <= 0.01f*0.01f) { PNewLocation = PCurrentLocation; bChangedPosition = false; } // Check if the new rotation is worthy of change PxQuat PNewOrientation = U2PQuat(NewTransform.GetRotation()); PxQuat PCurrentOrientation = KinActor->getGlobalPose().q; if(!(FMath::Abs(PNewOrientation.dot(PCurrentOrientation)) < (1.f - KINDA_SMALL_NUMBER))) { PNewOrientation = PCurrentOrientation; bChangedRotation = false; } // Don't call moveKinematic if it hasn't changed - that will stop bodies from going to sleep. if (bChangedPosition || bChangedRotation) { KinActor->setKinematicTarget(PxTransform(PNewLocation, PNewOrientation)); //LOC_MOD //PxD6Joint* Joint = (PxD6Joint*) HandleData; //if(Joint)// && (PNewLocation - PCurrentLocation).magnitudeSquared() > 0.01f*0.01f) //{ // PxRigidActor* Actor0, *Actor1; // Joint->getActors(Actor0, Actor1); // //Joint->setDrivePosition(PxTransform(Actor0->getGlobalPose().transformInv(PNewLocation))); // Joint->setDrivePosition(PxTransform::createIdentity()); // //Joint->setDriveVelocity(PxVec3(0), PxVec3(0)); //} } #endif // WITH_PHYSX }
static bool updateMassAndInertia(bool multipleMassOrDensity, PxRigidBody& body, const PxReal* densities, PxU32 densityCount, const PxVec3* massLocalPose, bool includeNonSimShapes) { bool success; // default values in case there were no shapes PxReal massOut = 1.0f; PxVec3 diagTensor(1.f,1.f,1.f); PxQuat orient = PxQuat(PxIdentity); bool lockCom = massLocalPose != NULL; PxVec3 com = lockCom ? *massLocalPose : PxVec3(0); const char* errorStr = "PxRigidBodyExt::updateMassAndInertia"; if (densities && densityCount) { Ext::InertiaTensorComputer inertiaComp(true); if(computeMassAndInertia(multipleMassOrDensity, body, densities, NULL, densityCount, includeNonSimShapes, inertiaComp)) { if(inertiaComp.getMass()!=0 && computeMassAndDiagInertia(inertiaComp, diagTensor, orient, massOut, com, lockCom, body, errorStr)) success = true; else success = false; // body with no shapes provided or computeMassAndDiagInertia() failed } else { Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "%s: Mass and inertia computation failed, setting mass to 1 and inertia to (1,1,1)", errorStr); success = false; } } else { Ps::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, "%s: No density specified, setting mass to 1 and inertia to (1,1,1)", errorStr); success = false; } PX_ASSERT(orient.isFinite()); PX_ASSERT(diagTensor.isFinite()); PX_ASSERT(PxIsFinite(massOut)); body.setMass(massOut); body.setMassSpaceInertiaTensor(diagTensor); body.setCMassLocalPose(PxTransform(com, orient)); return success; }
PxQuat slerp(const PxReal t, const PxQuat& left, const PxQuat& right) { const PxReal quatEpsilon = (PxReal(1.0e-8f)); PxReal cosine = left.dot(right); PxReal sign = PxReal(1); if (cosine < 0) { cosine = -cosine; sign = PxReal(-1); } PxReal sine = PxReal(1) - cosine*cosine; if(sine>=quatEpsilon*quatEpsilon) { sine = PxSqrt(sine); const PxReal angle = PxAtan2(sine, cosine); const PxReal i_sin_angle = PxReal(1) / sine; const PxReal leftw = PxSin(angle*(PxReal(1)-t)) * i_sin_angle; const PxReal rightw = PxSin(angle * t) * i_sin_angle * sign; return left * leftw + right * rightw; } return left; }
void BasicRandom::unitRandomQuat(PxQuat& v) { v.x = randomFloat(); v.y = randomFloat(); v.z = randomFloat(); v.w = randomFloat(); v.normalize(); }
void UGripMotionControllerComponent::UpdatePhysicsHandleTransform(const FBPActorGripInformation &GrippedActor, const FTransform& NewTransform) { if (!GrippedActor.Actor && !GrippedActor.Component) return; FBPActorPhysicsHandleInformation * HandleInfo = GetPhysicsGrip(GrippedActor); if (!HandleInfo || !HandleInfo->KinActorData) return; #if WITH_PHYSX bool bChangedPosition = true; bool bChangedRotation = true; PxRigidDynamic* KinActor = HandleInfo->KinActorData; PxScene* PScene = GetPhysXSceneFromIndex(HandleInfo->SceneIndex); SCOPED_SCENE_WRITE_LOCK(PScene); // Check if the new location is worthy of change PxVec3 PNewLocation = U2PVector(NewTransform.GetTranslation()); PxVec3 PCurrentLocation = KinActor->getGlobalPose().p; if ((PNewLocation - PCurrentLocation).magnitudeSquared() <= 0.01f*0.01f) { PNewLocation = PCurrentLocation; bChangedPosition = false; } // Check if the new rotation is worthy of change PxQuat PNewOrientation = U2PQuat(NewTransform.GetRotation()); PxQuat PCurrentOrientation = KinActor->getGlobalPose().q; if ((FMath::Abs(PNewOrientation.dot(PCurrentOrientation)) > (1.f - SMALL_NUMBER))) { PNewOrientation = PCurrentOrientation; bChangedRotation = false; } // Don't call moveKinematic if it hasn't changed - that will stop bodies from going to sleep. if (bChangedPosition || bChangedRotation) { KinActor->setKinematicTarget(PxTransform(PNewLocation, PNewOrientation)); } #endif // WITH_PHYSX }
bool Character::buildMotion(Acclaim::AMCData &amcData, Motion &motion, PxU32 start, PxU32 end) { using namespace Acclaim; if (mASFData == NULL) return false; motion.mNbFrames = end - start + 1; motion.mMotionData = SAMPLE_NEW(MotionData)[motion.mNbFrames]; // compute bounds of all the motion data on normalized frame PxBounds3 bounds = PxBounds3::empty(); for (PxU32 i = start; i < end; i++) { Acclaim::FrameData &frameData = amcData.mFrameData[i]; PxTransform rootTransform(PxVec3(0.0f), EulerAngleToQuat(frameData.mRootOrientation)); for (PxU32 j = 0; j < mASFData->mNbBones; j++) { PxTransform t = computeBoneTransform(rootTransform, mASFData->mBones[j], frameData.mBoneFrameData); bounds.include(t.p); } } Acclaim::FrameData& firstFrame = amcData.mFrameData[0]; Acclaim::FrameData& lastFrame = amcData.mFrameData[amcData.mNbFrames-1]; // compute direction vector motion.mDistance = mCharacterScale * (lastFrame.mRootPosition - firstFrame.mRootPosition).magnitude(); PxVec3 firstPosition = firstFrame.mRootPosition; PX_UNUSED(firstPosition); PxVec3 firstAngles = firstFrame.mRootOrientation; PxQuat firstOrientation = EulerAngleToQuat(PxVec3(0, firstAngles.y, 0)); for (PxU32 i = 0; i < motion.mNbFrames; i++) { Acclaim::FrameData& frameData = amcData.mFrameData[i+start]; MotionData &motionData = motion.mMotionData[i]; // normalize y-rot by computing inverse quat from first frame // this makes all the motion aligned in the same (+ z) direction. PxQuat currentOrientation = EulerAngleToQuat(frameData.mRootOrientation); PxQuat qdel = firstOrientation.getConjugate() * currentOrientation; PxTransform rootTransform(PxVec3(0.0f), qdel); for (PxU32 j = 0; j < mNbBones; j++) { PxTransform boneTransform = computeBoneTransform(rootTransform, mASFData->mBones[j], frameData.mBoneFrameData); motionData.mBoneTransform[j] = boneTransform; } //PxReal y = mCharacterScale * (frameData.mRootPosition.y - firstPosition.y) - bounds.minimum.y; motionData.mRootTransform = PxTransform(PxVec3(0.0f, -bounds.minimum.y, 0.0f), PxQuat(PxIdentity)); } // now make the motion cyclic by linear interpolating root position and joint angles const PxU32 windowSize = 10; for (PxU32 i = 0; i <= windowSize; i++) { PxU32 j = motion.mNbFrames - 1 - windowSize + i; PxReal t = PxReal(i) / PxReal(windowSize); MotionData& motion_i = motion.mMotionData[0]; MotionData& motion_j = motion.mMotionData[j]; // lerp root translation PxVec3 blendedRootPos = (1.0f - t) * motion_j.mRootTransform.p + t * motion_i.mRootTransform.p; for (PxU32 k = 0; k < mNbBones; k++) { PxVec3 pj = motion_j.mRootTransform.p + motion_j.mBoneTransform[k].p; PxVec3 pi = motion_i.mRootTransform.p + motion_i.mBoneTransform[k].p; PxVec3 p = (1.0f - t) * pj + t * pi; motion_j.mBoneTransform[k].p = p - blendedRootPos; } motion_j.mRootTransform.p = blendedRootPos; } return true; }
void SampleCustomGravityCameraController::update(Camera& camera, PxReal dtime) { const PxExtendedVec3& currentPos = mCCT.getPosition(); const PxVec3 curPos = toVec3(currentPos); // Compute up vector for current CCT position PxVec3 upVector; mBase.mPlanet.getUpVector(upVector, curPos); PX_ASSERT(upVector.isFinite()); // Update CCT if(!mBase.isPaused()) { if(1) { bool recordPos = true; const PxU32 currentSize = mNbRecords; if(currentSize) { const PxVec3 lastPos = mHistory[currentSize-1]; // const float limit = 0.1f; const float limit = 0.5f; if((curPos - lastPos).magnitude()<limit) recordPos = false; } if(recordPos) { if(mNbRecords==POS_HISTORY_LIMIT) { for(PxU32 i=1;i<mNbRecords;i++) mHistory[i-1] = mHistory[i]; mNbRecords--; } mHistory[mNbRecords++] = curPos; } } // Subtract off the 'up' component of the view direction to get our forward motion vector. PxVec3 viewDir = camera.getViewDir(); PxVec3 forward = (viewDir - upVector * upVector.dot(viewDir)).getNormalized(); // PxVec3 forward = mForward; // Compute "right" vector PxVec3 right = forward.cross(upVector); right.normalize(); // PxVec3 right = mRightV; PxVec3 targetKeyDisplacement(0); if(mFwd) targetKeyDisplacement += forward; if(mBwd) targetKeyDisplacement -= forward; if(mRight) targetKeyDisplacement += right; if(mLeft) targetKeyDisplacement -= right; targetKeyDisplacement *= mKeyShiftDown ? mRunningSpeed : mWalkingSpeed; // targetKeyDisplacement += PxVec3(0,-9.81,0); targetKeyDisplacement *= dtime; PxVec3 targetPadDisplacement(0); targetPadDisplacement += forward * mGamepadForwardInc * mRunningSpeed; targetPadDisplacement += right * mGamepadLateralInc * mRunningSpeed; // targetPadDisplacement += PxVec3(0,-9.81,0); targetPadDisplacement *= dtime; const PxF32 heightDelta = gJump.getHeight(dtime); // printf("%f\n", heightDelta); PxVec3 upDisp = upVector; if(heightDelta!=0.0f) upDisp *= heightDelta; else upDisp *= -9.81f * dtime; const PxVec3 disp = targetKeyDisplacement + targetPadDisplacement + upDisp; //upDisp.normalize(); //printf("%f | %f | %f\n", upDisp.x, upDisp.y, upDisp.z); // printf("%f | %f | %f\n", targetKeyDisplacement.x, targetKeyDisplacement.y, targetKeyDisplacement.z); // printf("%f | %f | %f\n\n", targetPadDisplacement.x, targetPadDisplacement.y, targetPadDisplacement.z); mCCT.setUpDirection(upVector); const PxU32 flags = mCCT.move(disp, 0.001f, dtime, PxControllerFilters()); if(flags & PxControllerFlag::eCOLLISION_DOWN) { gJump.stopJump(); // printf("Stop jump\n"); } } // Update camera if(1) { mTargetYaw += mGamepadYawInc * dtime; mTargetPitch += mGamepadPitchInc * dtime; // Clamp pitch // if(mTargetPitch<mPitchMin) mTargetPitch = mPitchMin; // if(mTargetPitch>mPitchMax) mTargetPitch = mPitchMax; } if(1) { PxVec3 up = upVector; PxQuat localPitchQ(mTargetPitch, PxVec3(1.0f, 0.0f, 0.0f)); PX_ASSERT(localPitchQ.isSane()); PxMat33 localPitchM(localPitchQ); const PxVec3 upRef(0.0f, 1.0f, 0.0f); PxQuat localYawQ(mTargetYaw, upRef); PX_ASSERT(localYawQ.isSane()); PxMat33 localYawM(localYawQ); bool res; PxQuat localToWorldQ = rotationArc(upRef, up, res); static PxQuat memory(0,0,0,1); if(!res) { localToWorldQ = memory; } else { memory = localToWorldQ; } PX_ASSERT(localToWorldQ.isSane()); PxMat33 localToWorld(localToWorldQ); static PxVec3 previousUp(0.0f, 1.0f, 0.0f); static PxQuat incLocalToWorldQ(0.0f, 0.0f, 0.0f, 1.0f); PxQuat incQ = rotationArc(previousUp, up, res); PX_ASSERT(incQ.isSane()); // incLocalToWorldQ = incLocalToWorldQ * incQ; incLocalToWorldQ = incQ * incLocalToWorldQ; PX_ASSERT(incLocalToWorldQ.isSane()); incLocalToWorldQ.normalize(); PxMat33 incLocalToWorldM(incLocalToWorldQ); localToWorld = incLocalToWorldM; previousUp = up; mTest = localToWorld; //mTest = localToWorld * localYawM; // PxMat33 rot = localYawM * localToWorld; PxMat33 rot = localToWorld * localYawM * localPitchM; // PxMat33 rot = localToWorld * localYawM; PX_ASSERT(rot.column0.isFinite()); PX_ASSERT(rot.column1.isFinite()); PX_ASSERT(rot.column2.isFinite()); //// PxMat44 view(rot.column0, rot.column1, rot.column2, PxVec3(0)); mForward = -rot.column2; mRightV = rot.column0; camera.setView(PxTransform(view)); PxVec3 viewDir = camera.getViewDir(); PX_ASSERT(viewDir.isFinite()); //// PxRigidActor* characterActor = mCCT.getActor(); PxShape* shape; characterActor->getShapes(&shape,1); PxCapsuleGeometry geom; shape->getCapsuleGeometry(geom); up *= geom.halfHeight+geom.radius; const PxVec3 headPos = curPos + up; const float distanceToTarget = 10.0f; // const float distanceToTarget = 20.0f; // const float distanceToTarget = 5.0f; // const PxVec3 camPos = headPos - viewDir*distanceToTarget; const PxVec3 camPos = headPos - mForward*distanceToTarget;// + up * 20.0f; // view.t = camPos; view.column3 = PxVec4(camPos,0); // camera.setView(view); camera.setView(PxTransform(view)); mTarget = headPos; } if(0) { PxControllerState cctState; mCCT.getState(cctState); printf("\nCCT state:\n"); printf("delta: %.02f | %.02f | %.02f\n", cctState.deltaXP.x, cctState.deltaXP.y, cctState.deltaXP.z); printf("touchedShape: %p\n", cctState.touchedShape); printf("touchedObstacle: %p\n", cctState.touchedObstacle); printf("standOnAnotherCCT: %d\n", cctState.standOnAnotherCCT); printf("standOnObstacle: %d\n", cctState.standOnObstacle); printf("isMovingUp: %d\n", cctState.isMovingUp); printf("collisionFlags: %d\n", cctState.collisionFlags); } }
void NpArticulationJoint::setTargetOrientation(const PxQuat& p) { PX_CHECK_AND_RETURN(mJoint.getDriveType() != PxArticulationJointDriveType::eTARGET || p.isUnit(), "NpArticulationJoint::setTargetOrientation, quat orientation is not valid."); PX_CHECK_AND_RETURN(mJoint.getDriveType() != PxArticulationJointDriveType::eERROR || (p.getImaginaryPart().isFinite() && p.w == 0), "NpArticulationJoint::setTargetOrientation rotation vector orientation is not valid."); NP_WRITE_CHECK(getOwnerScene()); mJoint.setTargetOrientation(p); }
/** @brief update vertex position @date 2014-02-26 */ void RendererBezierShape::SetBezierCurve(const vector<PxVec3> &points, const PxVec3 &color)//color=PxVec(0,0,0) { const PxU32 numVerts = (NODE_COUNT-1)*CONER_COUNT + 1; // +1 is head vertex point const PxVec3 diffuse = color * 255; const PxU32 diffuseColor = m_renderer.convertColor(RendererColor(diffuse.x, diffuse.y, diffuse.z)); RENDERER_ASSERT(m_vertexBuffer, "Failed to create Vertex Buffer."); if (m_vertexBuffer) { PxU32 stride = 0; void* vertPositions = m_vertexBuffer->lockSemantic(RendererVertexBuffer::SEMANTIC_POSITION, stride); //void *colors = m_vertexBuffer->lockSemantic(RendererVertexBuffer::SEMANTIC_COLOR, stride); void *normals = m_vertexBuffer->lockSemantic(RendererVertexBuffer::SEMANTIC_NORMAL, stride); PxVec3 oldPos; int vtxOffset = 0; for (int i=0; i < NODE_COUNT-1; ++i) { PxVec3 pos; utility::bezier(pos, points, (float)i/(float)(NODE_COUNT-1)); PxQuat q; if (i > 0) { PxVec3 curve = pos - oldPos; curve.normalize(); utility::quatRotationArc(q, PxVec3(1,0,0), curve); } float DEPTH = .03f; if (i == NODE_COUNT-2) DEPTH += DEPTH*3; // arrow head for (int k=0; k < CONER_COUNT; ++k) { const float rad = -PxPi * 2.f * ((float)k/(float)CONER_COUNT); PxVec3 dir = PxQuat(rad, PxVec3(1,0,0)).rotate(PxVec3(0,1,0)); if (i > 0) dir = q.rotate(dir); *(PxVec3*)(((PxU8*)vertPositions) + vtxOffset) = pos + dir*DEPTH; *(PxVec3*)(((PxU8*)normals) + vtxOffset) = PxVec3(0,1,0); //*(PxU32*)(((PxU8*)colors) + vtxOffset) = diffuseColor; vtxOffset += stride; } // arrow head DEPTH += DEPTH; // head length if (i == NODE_COUNT-2) { utility::bezier(pos, points, 1); if ((pos-oldPos).magnitude() < 0.2f) { PxVec3 v0 = pos-oldPos; if (!v0.isZero()) { v0.normalize(); pos = oldPos + v0*0.2f; } } PxVec3 dir = q.rotate(PxVec3(1,0,0)); *(PxVec3*)(((PxU8*)vertPositions) + vtxOffset) = pos; *(PxVec3*)(((PxU8*)normals) + vtxOffset) = PxVec3(0,1,0); //*(PxU32*)(((PxU8*)colors) + vtxOffset) = diffuseColor; } oldPos = pos; } m_vertexBuffer->unlockSemantic(RendererVertexBuffer::SEMANTIC_POSITION); //m_vertexBuffer->unlockSemantic(RendererVertexBuffer::SEMANTIC_COLOR); m_vertexBuffer->unlockSemantic(RendererVertexBuffer::SEMANTIC_NORMAL); } }
/** @brief MouseRButtonUp @date 2014-03-04 */ void COrientationEditController::MouseRButtonUp(physx::PxU32 x, physx::PxU32 y) { switch (m_editMode) { case MODE_POSITION: { RET(!m_selectNode); using namespace genotype_parser; SConnection *joint = m_rootNode->GetJoint(m_selectNode); RET(!joint); PxVec3 dir = m_selectNode->GetPos() - m_rootNode->GetPos(); const float len = dir.magnitude(); dir.normalize(); PxQuat rotateToXAxis; if (boost::iequals(joint->type, "revolute")) { PxVec3 jointAxis = PxVec3(0,0,1).cross(dir); jointAxis.normalize(); utility::quatRotationArc(rotateToXAxis, PxVec3(1,0,0), jointAxis); } else { rotateToXAxis = m_rootNode->GetWorldTransform().q; } { // setting parent orientation PxReal angle; PxVec3 axis; rotateToXAxis.toRadiansAndUnitAxis(angle, axis); joint->parentOrient.angle = angle; joint->parentOrient.dir = utility::PxVec3toVec3(axis); printf( "parent angle = %f, dir= %f %f %f\n", angle, axis.x, axis.y, axis.z); } { // setting select orientation PxVec3 pos; PxTransform rotTm; const PxTransform tm = m_selectNode->GetWorldTransform(); if (boost::iequals(joint->type, "revolute")) { rotTm = PxTransform(rotateToXAxis) * PxTransform(tm.q); pos = rotTm.rotate(dir*len); } else { rotTm = PxTransform(tm.q); pos = tm.p; } PxReal angle; PxVec3 axis; rotTm.q.toRadiansAndUnitAxis(angle, axis); joint->orient.angle = angle; joint->orient.dir = utility::PxVec3toVec3(axis); joint->pos = utility::PxVec3toVec3(pos); printf( "connect angle = %f, dir= %f %f %f\n", angle, axis.x, axis.y, axis.z); } SelectNode(NULL); } break; } if (m_cursor) m_cursor->MouseRButtonUp(x,y); }