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
}
void defaultCCTInteraction(const PxControllerShapeHit& hit)
{
PxRigidDynamic* actor = hit.shape->getActor()->is<PxRigidDynamic>();
if(actor)
{
if(actor->getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC)
return;
if(0)
{
const PxVec3 p = actor->getGlobalPose().p + hit.dir * 10.0f;
PxShape* shape;
actor->getShapes(&shape, 1);
PxRaycastHit newHit;
PxU32 n = PxShapeExt::raycast(*shape, *shape->getActor(), p, -hit.dir, 20.0f, PxHitFlag::ePOSITION, 1, &newHit, false);
if(n)
{
// We only allow horizontal pushes. Vertical pushes when we stand on dynamic objects creates
// useless stress on the solver. It would be possible to enable/disable vertical pushes on
// particular objects, if the gameplay requires it.
const PxVec3 upVector = hit.controller->getUpDirection();
const PxF32 dp = hit.dir.dot(upVector);
// shdfnd::printFormatted("%f\n", fabsf(dp));
if(fabsf(dp)<1e-3f)
// if(hit.dir.y==0.0f)
{
const PxTransform globalPose = actor->getGlobalPose();
const PxVec3 localPos = globalPose.transformInv(newHit.position);
::addForceAtLocalPos(*actor, hit.dir*hit.length*1000.0f, localPos, PxForceMode::eACCELERATION);
}
}
}
// We only allow horizontal pushes. Vertical pushes when we stand on dynamic objects creates
// useless stress on the solver. It would be possible to enable/disable vertical pushes on
// particular objects, if the gameplay requires it.
const PxVec3 upVector = hit.controller->getUpDirection();
const PxF32 dp = hit.dir.dot(upVector);
// shdfnd::printFormatted("%f\n", fabsf(dp));
if(fabsf(dp)<1e-3f)
// if(hit.dir.y==0.0f)
{
const PxTransform globalPose = actor->getGlobalPose();
const PxVec3 localPos = globalPose.transformInv(toVec3(hit.worldPos));
::addForceAtLocalPos(*actor, hit.dir*hit.length*1000.0f, localPos, PxForceMode::eACCELERATION);
}
}
}
void AddRadialForceToPxRigidDynamic(PxRigidDynamic& PRigidDynamic, const FVector& Origin, float Radius, float Strength, uint8 Falloff)
{
#if WITH_PHYSX
if (!(PRigidDynamic.getRigidDynamicFlags() & PxRigidDynamicFlag::eKINEMATIC))
{
float Mass = PRigidDynamic.getMass();
PxTransform PCOMTransform = PRigidDynamic.getGlobalPose().transform(PRigidDynamic.getCMassLocalPose());
PxVec3 PCOMPos = PCOMTransform.p; // center of mass in world space
PxVec3 POrigin = U2PVector(Origin); // origin of radial impulse, in world space
PxVec3 PDelta = PCOMPos - POrigin; // vector from
float Mag = PDelta.magnitude(); // Distance from COM to origin, in Unreal scale : @todo: do we still need conversion scale?
// If COM is outside radius, do nothing.
if (Mag > Radius)
{
return;
}
PDelta.normalize();
// If using linear falloff, scale with distance.
float ForceMag = Strength;
if (Falloff == RIF_Linear)
{
ForceMag *= (1.0f - (Mag / Radius));
}
// Apply force
PxVec3 PImpulse = PDelta * ForceMag;
PRigidDynamic.addForce(PImpulse, PxForceMode::eFORCE);
}
#endif // WITH_PHYSX
}
void AddRadialImpulseToPxRigidDynamic(PxRigidDynamic& PRigidDynamic, const FVector& Origin, float Radius, float Strength, uint8 Falloff, bool bVelChange)
{
#if WITH_PHYSX
if (!(PRigidDynamic.getRigidDynamicFlags() & PxRigidDynamicFlag::eKINEMATIC))
{
float Mass = PRigidDynamic.getMass();
PxTransform PCOMTransform = PRigidDynamic.getGlobalPose().transform(PRigidDynamic.getCMassLocalPose());
PxVec3 PCOMPos = PCOMTransform.p; // center of mass in world space
PxVec3 POrigin = U2PVector(Origin); // origin of radial impulse, in world space
PxVec3 PDelta = PCOMPos - POrigin; // vector from origin to COM
float Mag = PDelta.magnitude(); // Distance from COM to origin, in Unreal scale : @todo: do we still need conversion scale?
// If COM is outside radius, do nothing.
if (Mag > Radius)
{
return;
}
PDelta.normalize();
// Scale by U2PScale here, because units are velocity * mass.
float ImpulseMag = Strength;
if (Falloff == RIF_Linear)
{
ImpulseMag *= (1.0f - (Mag / Radius));
}
PxVec3 PImpulse = PDelta * ImpulseMag;
PxForceMode::Enum Mode = bVelChange ? PxForceMode::eVELOCITY_CHANGE : PxForceMode::eIMPULSE;
PRigidDynamic.addForce(PImpulse, Mode);
}
#endif // WITH_PHYSX
}
void getSimplePose( PxActor* actor, float* data ) //TODO rework
{
PxShape* shp[1];
PxRigidDynamic* rigid = (PxRigidDynamic*)actor;
rigid->getShapes( shp, PxU32(1) );
PxMat44 shape_pose = rigid->getGlobalPose(); //(PxShapeExt::getGlobalPose(*shp[0], *rigid));
for( int i = 0; i < 4; i++ )
for( int j = 0; j < 4; j++ )
data[i*4 + j] = shape_pose[j][i];
}
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
}
void setup(){
ofSetFrameRate(150);
w=ofGetScreenWidth();
h=ofGetScreenHeight();
InitPhysX();
for(int i=0; i<=300; i++) {
if(gScene) StepPhysX();
PxVec3 boxPos = gBox->getGlobalPose().p;
cout<<"BoxPosition: X:"<<boxPos.x <<" - Y:"<<boxPos.y <<" - Z:"<<boxPos.z<<"\n";
}
}
void
Spacetime::saveState(void) {
linearVelocityVector.clear();
angularVelocityVector.clear();
globalPoseVector.clear();
for (int i = 0; i < dynamic_actors.size(); i++) {
PxRigidDynamic* current = dynamic_actors[i];
linearVelocityVector.push_back(current->getLinearVelocity());
angularVelocityVector.push_back(current->getAngularVelocity());
globalPoseVector.push_back(current->getGlobalPose());
}
}
void PhysicsSystemPhysX::update(const Time& deltaTime)
{
// Step the simulation forward
mScene->simulate(deltaTime.seconds());
if (mScene->fetchResults())
{
// Now let's push back the transforms into the World
ComponentManager* colliderBoxManager = getWorld()->getComponentManager<CColliderBox>();
ComponentManager* transformManager = getWorld()->getComponentManager<CTransform>();
for (std::size_t i = 0; i < colliderBoxManager->size(); ++i)
{
Entity E = colliderBoxManager->getInstanceEntity(i);
CColliderBox* box = (CColliderBox*)colliderBoxManager->getInstance(i);
CTransform* transform = (CTransform*)transformManager->getComponentFromEntity(E);
// Create
if (box->userData == nullptr)
{
PxMaterial* boxMaterial = mPhysics->createMaterial(0.5f, 0.5f, 0.1f);
PxRigidDynamic* boxActor = mPhysics->createRigidDynamic(PxTransform(transform->position.x, transform->position.y, transform->position.z));
boxActor->createShape(PxBoxGeometry(10.f, 10.f, 10.f), *boxMaterial);
PxRigidBodyExt::updateMassAndInertia(*boxActor, 30);
//boxActor->setLinearVelocity(PxVec3(0, -50, 0));
mScene->addActor(*boxActor);
box->userData = boxActor;
}
// Update
else
{
PxRigidDynamic* boxActor = (PxRigidDynamic*)box->userData;
PxTransform pxTransform = boxActor->getGlobalPose();
transform->position.x = pxTransform.p.x;
transform->position.y = pxTransform.p.y;
transform->position.z = pxTransform.p.z;
transform->rotation.x = pxTransform.q.x;
transform->rotation.y = pxTransform.q.y;
transform->rotation.z = pxTransform.q.z;
transform->rotation.w = pxTransform.q.w;
}
}
syncActors();
}
}
void ScoreCallback::onTrigger(PxTriggerPair* pairs, PxU32 count)
{
for (int i = 0; i < count; i++)
{
if (pairs[i].otherActor->isRigidDynamic() && pairs[i].otherActor->getName() == "Ball")
{
if (pairs[i].status & PxPairFlag::eNOTIFY_TOUCH_FOUND)
{
PxRigidDynamic* ball = static_cast<PxRigidDynamic*>(pairs[i].otherActor);
if (pairs[i].triggerActor->getName() == "BumperHigh" || pairs[i].triggerActor->getName() == "BumperLow")
{
PxRigidDynamic* bumper = static_cast<PxRigidDynamic*>(pairs[i].triggerActor);
if (pairs[i].triggerActor->getName() == "BumperHigh")
Pinball::AddScoreHigh = true;
else
Pinball::AddScoreLow = true;
Transform bumperPose = bumper->getGlobalPose();
Transform ballPose = ball->getGlobalPose();
Vec3 dir = bumperPose.p - ballPose.p;
dir.normalize();
dir = Vec3(dir.x, dir.y, dir.z);
Pinball::BounceBall = true;
Pinball::BallBounceDirection = dir;
}
if (pairs[i].triggerActor->getName() == "SpinnerSwitch")
{
Pinball::EnableSpinners = true;
}
}
}
}
}
PxReal labscale::getRegolithSurface()
{
PxReal regSurf = -PX_MAX_REAL;
PxU32 nbActors = gPhysX.mScene->getActors(gPhysX.roles.dynamics,gPhysX.cast,MAX_ACTORS_PER_SCENE);
while (nbActors--)
{
PxRigidDynamic* actor = gPhysX.cast[nbActors]->isRigidDynamic();
if (actor && strcmp(actor->getName(), "regolith") == 0)
{
PxReal y = actor->getGlobalPose().p.y;
if (y>regSurf) regSurf = y;
}
}
return (regSurf + labscale::regolith.diameter/2.0);
}
void PhysX::AddLocalTorque(PintObjectHandle handle, const Point& local_torque)
{
PxRigidActor* RigidActor = GetActorFromHandle(handle);
if(!RigidActor)
{
PxShape* Shape = GetShapeFromHandle(handle);
ASSERT(Shape);
RigidActor = &Shape->getActor();
}
if(RigidActor->getConcreteType()==PxConcreteType::eRIGID_DYNAMIC)
{
PxRigidDynamic* RigidDynamic = static_cast<PxRigidDynamic*>(RigidActor);
const PxVec3 GlobalTorque = RigidDynamic->getGlobalPose().rotate(ToPxVec3(local_torque));
RigidDynamic->addTorque(GlobalTorque, PxForceMode::eACCELERATION, true);
}
}
void CSlingshotEngine::loadSlingshot()
{
Matrix44 transformation2;
setTranslation(transformation2, Vector3(0.0f, -2.5f, -15.0f));
Entity* pBird = SceneFactory::CreateBird(transformation2, EntityCategories::BIRD, 1.0f);
m_pBirdBody = pBird->getSingleComponent<PhysXBody>();
PxRigidDynamic* pBirdDynamic = m_pBirdBody->getActor()->isRigidDynamic();
Vector3 arm0Position = PhysXVector::toVector3(m_pArm0Body->getActor()->isRigidActor()->getGlobalPose().p);
Vector3 arm1Position = PhysXVector::toVector3(m_pArm1Body->getActor()->isRigidActor()->getGlobalPose().p);
Vector3 birdPosition = PhysXVector::toVector3(pBirdDynamic->getGlobalPose().p);
Vector3 arm0ToBird = birdPosition - arm0Position;
Matrix44 arm0LocalFrame;
setTranslation(arm0LocalFrame, arm0ToBird * 0.5f);
Vector3 arm1ToBird = birdPosition - arm1Position;
Matrix44 arm1LocalFrame;
setTranslation(arm1LocalFrame, arm1ToBird * 0.5f);
Matrix44 birdLocalFrame;
setTranslation(birdLocalFrame, arm0ToBird * -0.5f);
PxPhysics* pPhysics = static_cast<PhysXPhysicsFactory*>(PhysicsFactory::getInstance())->getPhysics();
m_pArm0Joint = PxD6JointCreate(*pPhysics, m_pArm0Body->getActor()->isRigidActor(),
PhysXMatrix::toPxTransform(arm0LocalFrame), pBirdDynamic, PhysXMatrix::toPxTransform(birdLocalFrame));
setTranslation(birdLocalFrame, arm1ToBird * -0.5f);
m_pArm1Joint = PxD6JointCreate(*pPhysics, m_pArm1Body->getActor()->isRigidActor(),
PhysXMatrix::toPxTransform(arm1LocalFrame), pBirdDynamic, PhysXMatrix::toPxTransform(birdLocalFrame));
m_pArm0Joint->setMotion(PxD6Axis::eX, PxD6Motion::eFREE);
m_pArm0Joint->setMotion(PxD6Axis::eY, PxD6Motion::eFREE);
m_pArm0Joint->setMotion(PxD6Axis::eZ, PxD6Motion::eFREE);
m_pArm1Joint->setMotion(PxD6Axis::eX, PxD6Motion::eFREE);
m_pArm1Joint->setMotion(PxD6Axis::eY, PxD6Motion::eFREE);
m_pArm1Joint->setMotion(PxD6Axis::eZ, PxD6Motion::eFREE);
PxD6JointDrive drive(500.0f, -20.0f, PX_MAX_F32);
m_pArm0Joint->setDrive(PxD6Drive::eX, drive);
m_pArm0Joint->setDrive(PxD6Drive::eY, drive);
m_pArm0Joint->setDrive(PxD6Drive::eZ, drive);
m_pArm1Joint->setDrive(PxD6Drive::eX, drive);
m_pArm1Joint->setDrive(PxD6Drive::eY, drive);
m_pArm1Joint->setDrive(PxD6Drive::eZ, drive);
}
void defaultCCTInteraction(const PxControllerShapeHit& hit)
{
PxRigidDynamic* actor = hit.shape->getActor().is<PxRigidDynamic>();
if(actor)
{
if(actor->getRigidDynamicFlags() & PxRigidDynamicFlag::eKINEMATIC)
return;
// We only allow horizontal pushes. Vertical pushes when we stand on dynamic objects creates
// useless stress on the solver. It would be possible to enable/disable vertical pushes on
// particular objects, if the gameplay requires it.
const PxVec3 upVector = hit.controller->getUpDirection();
const PxF32 dp = hit.dir.dot(upVector);
// printf("%f\n", fabsf(dp));
if(fabsf(dp)<1e-3f)
// if(hit.dir.y==0.0f)
{
const PxTransform globalPose = actor->getGlobalPose();
const PxVec3 localPos = globalPose.transformInv(toVec3(hit.worldPos));
::addForceAtLocalPos(*actor, hit.dir*hit.length*1000.0f, localPos, PxForceMode::eACCELERATION);
}
}
}
void UPhysicsHandleComponent::GrabComponent(UPrimitiveComponent* InComponent, FName InBoneName, FVector Location, bool bConstrainRotation)
{
// If we are already holding something - drop it first.
if(GrabbedComponent != NULL)
{
ReleaseComponent();
}
if(!InComponent)
{
return;
}
#if WITH_PHYSX
// Get the PxRigidDynamic that we want to grab.
FBodyInstance* BodyInstance = InComponent->GetBodyInstance(InBoneName);
if (!BodyInstance)
{
return;
}
PxRigidDynamic* Actor = BodyInstance->GetPxRigidDynamic();
if (!Actor)
return;
// Get the scene the PxRigidDynamic we want to grab is in.
PxScene* Scene = Actor->getScene();
check(Scene);
// Get transform of actor we are grabbing
PxVec3 KinLocation = U2PVector(Location);
PxTransform GrabbedActorPose = Actor->getGlobalPose();
PxTransform KinPose(KinLocation, GrabbedActorPose.q);
// set target and current, so we don't need another "Tick" call to have it right
TargetTransform = CurrentTransform = P2UTransform(KinPose);
// If we don't already have a handle - make one now.
if (!HandleData)
{
// Create kinematic actor we are going to create joint with. This will be moved around with calls to SetLocation/SetRotation.
PxRigidDynamic* KinActor = Scene->getPhysics().createRigidDynamic(KinPose);
KinActor->setRigidDynamicFlag(PxRigidDynamicFlag::eKINEMATIC, true);
KinActor->setMass(1.0f);
KinActor->setMassSpaceInertiaTensor(PxVec3(1.0f, 1.0f, 1.0f));
// No bodyinstance
KinActor->userData = NULL;
// Add to Scene
Scene->addActor(*KinActor);
// Save reference to the kinematic actor.
KinActorData = KinActor;
// Create the joint
PxVec3 LocalHandlePos = GrabbedActorPose.transformInv(KinLocation);
PxD6Joint* NewJoint = PxD6JointCreate(Scene->getPhysics(), KinActor, PxTransform::createIdentity(), Actor, PxTransform(LocalHandlePos));
if(!NewJoint)
{
HandleData = 0;
}
else
{
// No constraint instance
NewJoint->userData = NULL;
HandleData = NewJoint;
// Remember the scene index that the handle joint/actor are in.
FPhysScene* RBScene = FPhysxUserData::Get<FPhysScene>(Scene->userData);
const uint32 SceneType = InComponent->BodyInstance.UseAsyncScene() ? PST_Async : PST_Sync;
SceneIndex = RBScene->PhysXSceneIndex[SceneType];
// Setting up the joint
NewJoint->setMotion(PxD6Axis::eX, PxD6Motion::eFREE);
NewJoint->setMotion(PxD6Axis::eY, PxD6Motion::eFREE);
NewJoint->setMotion(PxD6Axis::eZ, PxD6Motion::eFREE);
NewJoint->setDrive(PxD6Drive::eX, PxD6JointDrive(LinearStiffness, LinearDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
NewJoint->setDrive(PxD6Drive::eY, PxD6JointDrive(LinearStiffness, LinearDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
NewJoint->setDrive(PxD6Drive::eZ, PxD6JointDrive(LinearStiffness, LinearDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
NewJoint->setDrivePosition(PxTransform(PxVec3(0,0,0)));
NewJoint->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
NewJoint->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
NewJoint->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);
bRotationConstrained = bConstrainRotation;
if (bRotationConstrained)
{
NewJoint->setDrive(PxD6Drive::eSLERP, PxD6JointDrive(AngularStiffness, AngularDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
//NewJoint->setDrive(PxD6Drive::eTWIST, PxD6JointDrive(AngularStiffness, AngularDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
//NewJoint->setDrive(PxD6Drive::eSWING, PxD6JointDrive(AngularStiffness, AngularDamping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
//PosJointDesc.setGlobalAxis(NxVec3(0,0,1));
}
}
}
#endif // WITH_PHYSX
GrabbedComponent = InComponent;
GrabbedBoneName = InBoneName;
}
DestructibleActorJointImpl::DestructibleActorJointImpl(const DestructibleActorJointDesc& desc, DestructibleScene& dscene) :
joint(NULL)
{
if (desc.destructible[0] == NULL && desc.destructible[1] == NULL)
{
APEX_DEBUG_WARNING("Both destructible actors in DestructibleActorJoint are NULL.");
return;
}
PxRigidActor* actor[2] = {desc.actor[0], desc.actor[1]};
PxVec3 localAxis[2] = {desc.localAxis[0], desc.localAxis[1]};
PxVec3 localAnchor[2] = {desc.localAnchor[0], desc.localAnchor[1]};
PxVec3 localNormal[2] = {desc.localNormal[0], desc.localNormal[1]};
PxTransform localFrame[2];
for (int i = 0; i < 2; ++i)
{
if (desc.destructible[i] == NULL)
{
structure[i] = NULL;
attachmentChunkIndex[i] = ModuleDestructibleConst::INVALID_CHUNK_INDEX;
if(NULL == actor[i])
{
// World constrained
PxMat33 rot(desc.globalAxis[i],desc.globalNormal[i],desc.globalAxis[i].cross(desc.globalNormal[i]));
localFrame[i].p = desc.globalAnchor[i];
localFrame[i].q = PxQuat(rot);
localFrame[i].q.normalize();
}
else
{
// Constrained to physics object
PxMat33 rot(localAxis[i], localNormal[i], localAxis[i].cross(localNormal[i]));
localFrame[i].p = localAnchor[i];
localFrame[i].q = PxQuat(rot);
localFrame[i].q.normalize();
}
continue;
}
PxRigidDynamic* attachActor = NULL;
DestructibleActorImpl& destructible = ((DestructibleActorProxy*)desc.destructible[i])->impl;
structure[i] = destructible.getStructure();
attachmentChunkIndex[i] = desc.attachmentChunkIndex[i];
if (attachmentChunkIndex[i] >= 0 && attachmentChunkIndex[i] < (int32_t)destructible.getDestructibleAsset()->getChunkCount())
{
DestructibleStructure::Chunk& chunk = structure[i]->chunks[destructible.getFirstChunkIndex() + attachmentChunkIndex[i]];
attachActor = structure[i]->dscene->chunkIntact(chunk);
}
SCOPED_PHYSX_LOCK_READ(dscene.getModulePhysXScene());
if (attachActor == NULL)
{
float minDistance = PX_MAX_F32;
for (uint32_t j = 0; j < destructible.getDestructibleAsset()->getChunkCount(); ++j)
{
DestructibleAssetParametersNS::Chunk_Type& source = destructible.getDestructibleAsset()->mParams->chunks.buf[j];
const bool hasChildren = source.numChildren != 0;
if (!hasChildren) // Only attaching to lowest-level chunks, initially
{
DestructibleStructure::Chunk& chunk = structure[i]->chunks[destructible.getFirstChunkIndex() + j];
PxRigidDynamic* actor = structure[i]->dscene->chunkIntact(chunk);
if (actor)
{
const float distance = (actor->getGlobalPose().transform(chunk.localSphereCenter) - desc.globalAnchor[i]).magnitude();
if (distance < minDistance)
{
attachActor = actor;
attachmentChunkIndex[i] = (int32_t)(destructible.getFirstChunkIndex() + j);
minDistance = distance;
}
}
}
}
}
if (attachActor == NULL)
{
APEX_DEBUG_WARNING("No physx actor could be found in destructible actor %p to attach the joint.", desc.destructible[i]);
return;
}
actor[i] = (PxRigidActor*)attachActor;
if (attachActor->getScene() != NULL && dscene.getModulePhysXScene() != attachActor->getScene())
{
APEX_DEBUG_WARNING("Trying to joint actors from a scene different from the joint scene.");
return;
}
localAnchor[i] = attachActor->getGlobalPose().transformInv(desc.globalAnchor[i]);
localAxis[i] = attachActor->getGlobalPose().rotateInv(desc.globalAxis[i]);
localNormal[i] = attachActor->getGlobalPose().rotateInv(desc.globalNormal[i]);
PxMat33 rot(localAxis[i], localNormal[i], localAxis[i].cross(localNormal[i]));
localFrame[i].p = localAnchor[i];
localFrame[i].q = PxQuat(rot);
localFrame[i].q.normalize();
}
dscene.getModulePhysXScene()->lockRead();
switch (desc.type)
{
case PxJointConcreteType::eD6:
joint = PxD6JointCreate(dscene.getModulePhysXScene()->getPhysics(), actor[0], localFrame[0], actor[1], localFrame[1]);
break;
case PxJointConcreteType::eDISTANCE:
joint = PxDistanceJointCreate(dscene.getModulePhysXScene()->getPhysics(), actor[0], localFrame[0], actor[1], localFrame[1]);
break;
case PxJointConcreteType::eFIXED:
joint = PxFixedJointCreate(dscene.getModulePhysXScene()->getPhysics(), actor[0], localFrame[0], actor[1], localFrame[1]);
break;
case PxJointConcreteType::ePRISMATIC:
joint = PxPrismaticJointCreate(dscene.getModulePhysXScene()->getPhysics(), actor[0], localFrame[0], actor[1], localFrame[1]);
break;
case PxJointConcreteType::eREVOLUTE:
joint = PxRevoluteJointCreate(dscene.getModulePhysXScene()->getPhysics(), actor[0], localFrame[0], actor[1], localFrame[1]);
break;
case PxJointConcreteType::eSPHERICAL:
joint = PxSphericalJointCreate(dscene.getModulePhysXScene()->getPhysics(), actor[0], localFrame[0], actor[1], localFrame[1]);
break;
default:
PX_ALWAYS_ASSERT();
break;
}
dscene.getModulePhysXScene()->unlockRead();
PX_ASSERT(joint != NULL);
}
physx::PxRigidDynamic* CPhysicManager::createDynamicSphere(const Vector3 &position, const float &radius, float mass, bool kinematic, bool trigger, int group, const Logic::Component::IPhysic *component)
{
assert(m_scene);
PxTransform pose(Vector3ToPxVec3(position));
PxSphereGeometry geom(radius);
PxMaterial *material = m_defaultMaterial;
float density = mass / ((4/3) * Common::Util::Math::PI * radius * radius * radius);
PxRigidDynamic *actor = nullptr;
if(kinematic) {
actor = PxCreateKinematic(*m_physics,pose,geom,*material,density);
} else {
actor = PxCreateDynamic(*m_physics,pose,geom,*material,density);
}
if(trigger) {
PxShape *shape;
actor->getShapes(&shape,1,0);
shape->setFlag(PxShapeFlag::eSIMULATION_SHAPE, false);
shape->setFlag(PxShapeFlag::eTRIGGER_SHAPE, true);
}
actor->userData = (void*)component;
PxSetGroup(*actor,group);
setupFiltering(actor,FilterGroup::eSPACE_FILTER,FilterGroup::eSPACE_FILTER);
PxD6Joint* joint = PxD6JointCreate(*m_physics, actor, PxTransform::createIdentity(), nullptr, actor->getGlobalPose());
joint->setMotion(PxD6Axis::eX,PxD6Motion::eFREE);
joint->setMotion(PxD6Axis::eY,PxD6Motion::eLOCKED);
joint->setMotion(PxD6Axis::eZ,PxD6Motion::eFREE);
joint->setMotion(PxD6Axis::eSWING1,PxD6Motion::eFREE);
joint->setMotion(PxD6Axis::eSWING2,PxD6Motion::eLOCKED);
joint->setMotion(PxD6Axis::eTWIST,PxD6Motion::eLOCKED);
//TODO release
//m_scene->addActor(*actor);
return actor;
}
void SampleSubmarine::onTickPreRender(float dtime)
{
// handle mine (and submarine) explosion
if(mSubmarineActor)
{
// explode all touched mines, apply damage and force to submarine
PxVec3 subMarinePos = mSubmarineActor->getGlobalPose().p;
const size_t nbExplodeSeamines = mMinesToExplode.size();
for(PxU32 i=0; i < nbExplodeSeamines; i++)
{
Seamine* mine = mMinesToExplode[i];
PxVec3 explosionPos = mine->mMineHead->getGlobalPose().p;
// disable contact trigger callback of the chain & enable gravity
const size_t nbLinks = mine->mLinks.size();
for(PxU32 j = 0; j < nbLinks; j++)
{
PxRigidDynamic* link = mine->mLinks[j];
setupFiltering(link, 0, 0);
link->setActorFlag(PxActorFlag::eDISABLE_GRAVITY, false);
}
// remove mine head
std::vector<Seamine*>::iterator mineIter = std::find(mSeamines.begin(), mSeamines.end(), mine);
if(mineIter != mSeamines.end())
mSeamines.erase(mineIter);
removeActor(mine->mMineHead);
delete mine;
// give damage to submarine
static const PxReal strength = 400.0f;
PxVec3 explosion = subMarinePos - explosionPos;
PxReal len = explosion.normalize();
PxReal damage = strength * (1.0f/len);
explosion *= damage;
mSubmarineActor->addForce(explosion*300);
gSubmarineHealth = PxMax(gSubmarineHealth-PxI32(damage), 0);
if(gSubmarineHealth == 0)
{
mSubmarineCameraController->init(subMarinePos - getCamera().getViewDir()*10.0f, getCamera().getRot());
explode(mSubmarineActor, subMarinePos /*- explosion*0.2f*/, damage);
mCameraAttachedToActor = NULL;
mSubmarineCameraController->setFollowingMode(false);
mSubmarineActor = NULL;
break;
}
}
mMinesToExplode.clear();
}
// respawn Crabs
const size_t nbCrabs = mCrabs.size();
for(PxU32 i = 0; i < nbCrabs; i++)
{
Crab* crab = mCrabs[i];
if(crab->needsRespawn())
{
PxRigidDynamic* prevBody = crab->getCrabBody();
PxVec3 prevPos = prevBody->getGlobalPose().p;
delete crab;
mCrabs[i] = SAMPLE_NEW(Crab)(*this, prevPos, mManagedMaterials[MATERIAL_RED]);
if(gCrab == crab)
gCrab = mCrabs[i];
if(mCameraAttachedToActor == prevBody)
mCameraAttachedToActor = mCrabs[i]->getCrabBody();
}
}
// update camera
if(mCameraAttachedToActor)
mSubmarineCameraController->updateFollowingMode(getCamera(), dtime, mCameraAttachedToActor->getGlobalPose().p);
// start the simulation
PhysXSample::onTickPreRender(dtime);
}