271 lines
7.7 KiB
C++
271 lines
7.7 KiB
C++
//
|
|
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions
|
|
// are met:
|
|
// * Redistributions of source code must retain the above copyright
|
|
// notice, this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright
|
|
// notice, this list of conditions and the following disclaimer in the
|
|
// documentation and/or other materials provided with the distribution.
|
|
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
|
// contributors may be used to endorse or promote products derived
|
|
// from this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
|
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
|
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
#include "SamplePreprocessor.h"
|
|
#include "KinematicPlatform.h"
|
|
#include "RendererMemoryMacros.h"
|
|
#include "PxScene.h"
|
|
#include "PxSceneLock.h"
|
|
#include "PxRigidDynamic.h"
|
|
#include "PxTkMatrixUtils.h"
|
|
|
|
PlatformState::PlatformState() :
|
|
mCurrentTime (0.0f),
|
|
mCurrentRotationTime(0.0f),
|
|
mFlip (false)
|
|
{
|
|
mPrevPose = PxTransform(PxIdentity);
|
|
}
|
|
|
|
KinematicPlatform::KinematicPlatform() :
|
|
mNbPts (0),
|
|
mPts (NULL),
|
|
mActor (NULL),
|
|
mBoxObstacle (NULL),
|
|
mObstacle (INVALID_OBSTACLE_HANDLE),
|
|
mTravelTime (0.0f),
|
|
mRotationSpeed (0.0f),
|
|
mMode (LOOP_FLIP)
|
|
{
|
|
}
|
|
|
|
KinematicPlatform::~KinematicPlatform()
|
|
{
|
|
DELETEARRAY(mPts);
|
|
}
|
|
|
|
void KinematicPlatform::release()
|
|
{
|
|
delete this;
|
|
}
|
|
|
|
void KinematicPlatform::init(PxU32 nbPts, const PxVec3* pts, const PxTransform& globalPose, const PxQuat& localRot, PxRigidDynamic* actor, PxReal travelTime, PxReal rotationSpeed, LoopMode mode)
|
|
{
|
|
DELETEARRAY(mPts);
|
|
mNbPts = nbPts;
|
|
mPts = SAMPLE_NEW(PxVec3Alloc)[nbPts];
|
|
PxVec3* dst = mPts;
|
|
for(PxU32 i=0;i<nbPts;i++)
|
|
dst[i] = globalPose.transform(pts[i]);
|
|
|
|
mLocalRot = localRot;
|
|
mActor = actor;
|
|
mTravelTime = travelTime;
|
|
mRotationSpeed = rotationSpeed;
|
|
mMode = mode;
|
|
PX_ASSERT(travelTime>0.0f);
|
|
}
|
|
|
|
void KinematicPlatform::setBoxObstacle(ObstacleHandle handle, const PxBoxObstacle* boxObstacle)
|
|
{
|
|
mObstacle = handle;
|
|
mBoxObstacle = boxObstacle;
|
|
}
|
|
|
|
PxU32 KinematicPlatform::getNbSegments() const
|
|
{
|
|
return mNbPts - 1;
|
|
}
|
|
|
|
PxReal KinematicPlatform::computeLength() const
|
|
{
|
|
const PxU32 nbSegments = getNbSegments();
|
|
|
|
float totalLength = 0.0f;
|
|
for(PxU32 i=0;i<nbSegments;i++)
|
|
{
|
|
const PxU32 a = i % mNbPts;
|
|
const PxU32 b = (i+1) % mNbPts;
|
|
totalLength += (mPts[b] - mPts[a]).magnitude();
|
|
}
|
|
return totalLength;
|
|
}
|
|
|
|
bool KinematicPlatform::getPoint(PxVec3& p, PxU32 seg, PxReal t) const
|
|
{
|
|
const PxU32 a = seg % mNbPts;
|
|
const PxU32 b = (seg+1) % mNbPts;
|
|
const PxVec3& p0 = mPts[a];
|
|
const PxVec3& p1 = mPts[b];
|
|
p = (1.0f - t) * p0 + t * p1;
|
|
return true;
|
|
}
|
|
|
|
bool KinematicPlatform::getPoint(PxVec3& p, PxReal t) const
|
|
{
|
|
// ### Not really optimized
|
|
|
|
const PxReal totalLength = computeLength();
|
|
const PxReal coeff = 1.0f / totalLength;
|
|
|
|
const PxU32 nbSegments = getNbSegments();
|
|
|
|
PxReal currentLength = 0.0f;
|
|
for(PxU32 i=0;i<nbSegments;i++)
|
|
{
|
|
const PxU32 a = i % mNbPts;
|
|
const PxU32 b = (i+1) % mNbPts;
|
|
const PxReal length = coeff * (mPts[b] - mPts[a]).magnitude();
|
|
|
|
if(t>=currentLength && t<=currentLength + length)
|
|
{
|
|
// Desired point is on current segment
|
|
// currentLength maps to 0.0
|
|
// currentLength+length maps to 1.0
|
|
const PxReal nt = (t-currentLength)/(length);
|
|
return getPoint(p, i, nt);
|
|
}
|
|
currentLength += length;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void KinematicPlatform::setT(PxF32 t)
|
|
{
|
|
if(t<0.0f || t>1.0f)
|
|
{
|
|
PX_ASSERT(0);
|
|
return;
|
|
}
|
|
|
|
const PxF32 curTime = mTravelTime*t;
|
|
|
|
mPhysicsState.mCurrentTime = curTime;
|
|
mRenderState.mCurrentTime = curTime;
|
|
}
|
|
|
|
void KinematicPlatform::updateState(PlatformState& state, PxObstacleContext* obstacleContext, PxReal dtime, bool updateActor) const
|
|
{
|
|
state.mCurrentTime += dtime;
|
|
state.mCurrentRotationTime += dtime;
|
|
|
|
// Compute current position on the path
|
|
PxReal t = state.mCurrentTime/mTravelTime;
|
|
if(t>1.0f)
|
|
{
|
|
if(mMode==LOOP_FLIP)
|
|
{
|
|
state.mFlip = !state.mFlip;
|
|
// Make it loop
|
|
state.mCurrentTime = fmodf(state.mCurrentTime, mTravelTime);
|
|
t = state.mCurrentTime/mTravelTime;
|
|
}
|
|
else
|
|
{
|
|
PX_ASSERT(mMode==LOOP_WRAP);
|
|
// state.mCurrentTime = fmodf(state.mCurrentTime, mTravelTime);
|
|
t = 1.0f - t;
|
|
state.mCurrentTime = t * mTravelTime;
|
|
}
|
|
}
|
|
|
|
PxVec3 currentPos;
|
|
if(getPoint(currentPos, state.mFlip ? 1.0f - t : t))
|
|
{
|
|
const PxVec3 wp = currentPos;
|
|
|
|
PxMat33 rotY;
|
|
PxToolkit::setRotX(rotY, state.mCurrentRotationTime*mRotationSpeed);
|
|
const PxQuat rotation(rotY);
|
|
|
|
const PxTransform tr(wp, mLocalRot * rotation);
|
|
|
|
//PxVec3 delta = wp - state.mPrevPos;
|
|
//shdfnd::printFormatted("Kine: %f | %f | %f\n", delta.x, delta.y, delta.z);
|
|
state.mPrevPose = tr;
|
|
if(updateActor)
|
|
{
|
|
PxSceneWriteLock scopedLock(*mActor->getScene());
|
|
mActor->setKinematicTarget(tr); // *
|
|
/*PxVec3 test = mActor->getGlobalPose().p;
|
|
test -= tr.p;
|
|
shdfnd::printFormatted("%f | %f | %f\n", test.x, test.y, test.z);*/
|
|
}
|
|
else if(obstacleContext && mBoxObstacle)
|
|
{
|
|
PxBoxObstacle localBox = *mBoxObstacle;
|
|
localBox.mPos.x = wp.x;
|
|
localBox.mPos.y = wp.y;
|
|
localBox.mPos.z = wp.z;
|
|
localBox.mRot = tr.q;
|
|
bool status = obstacleContext->updateObstacle(mObstacle, localBox);
|
|
PX_ASSERT(status);
|
|
PX_UNUSED(status);
|
|
}
|
|
}
|
|
}
|
|
|
|
void KinematicPlatform::resync()
|
|
{
|
|
mPhysicsState = mRenderState;
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
KinematicPlatformManager::KinematicPlatformManager() :
|
|
mElapsedPlatformTime(0.0f)
|
|
{
|
|
}
|
|
|
|
KinematicPlatformManager::~KinematicPlatformManager()
|
|
{
|
|
}
|
|
|
|
void KinematicPlatformManager::release()
|
|
{
|
|
const size_t nbPlatforms = mPlatforms.size();
|
|
for(PxU32 i=0;i<nbPlatforms;i++)
|
|
mPlatforms[i]->release();
|
|
mPlatforms.clear();
|
|
}
|
|
|
|
KinematicPlatform* KinematicPlatformManager::createPlatform(PxU32 nbPts, const PxVec3* pts, const PxTransform& pose, const PxQuat& localRot, PxRigidDynamic* actor, PxReal platformSpeed, PxReal rotationSpeed, LoopMode mode)
|
|
{
|
|
KinematicPlatform* kine = SAMPLE_NEW(KinematicPlatform);
|
|
kine->init(nbPts, pts, pose, localRot, actor, 1.0f, rotationSpeed, mode);
|
|
mPlatforms.push_back(kine);
|
|
|
|
const PxReal pathLength = kine->computeLength();
|
|
kine->setTravelTime(pathLength / platformSpeed);
|
|
return kine;
|
|
}
|
|
|
|
void KinematicPlatformManager::updatePhysicsPlatforms(float dtime)
|
|
{
|
|
// PT: keep track of time from the point of view of physics platforms.
|
|
// - if we call this each substep using fixed timesteps, it is never exactly in sync with the render time.
|
|
// - if we drop substeps because of the "well of despair", it can seriously lag behind the render time.
|
|
mElapsedPlatformTime += dtime;
|
|
|
|
// PT: compute new positions for (physics) platforms, then 'setKinematicTarget' their physics actors to these positions.
|
|
const size_t nbPlatforms = mPlatforms.size();
|
|
for(PxU32 i=0;i<nbPlatforms;i++)
|
|
mPlatforms[i]->updatePhysics(dtime);
|
|
}
|