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);
}
Esempio n. 2
0
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));
    }
}
Esempio n. 3
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;
}
Esempio n. 7
0
	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;
	}
Esempio n. 8
0
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);
}
Esempio n. 13
0
/**
 @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);
}