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