109 lines
3.5 KiB
C++
109 lines
3.5 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 "TestMotionGenerator.h"
|
|
|
|
//----------------------------------------------------------------------------//
|
|
MotionGenerator::MotionGenerator()
|
|
: mLinear(0.0f),
|
|
mAngular(0.0f),
|
|
mTransform(PxTransform(PxIdentity))
|
|
{
|
|
}
|
|
|
|
//----------------------------------------------------------------------------//
|
|
MotionGenerator::MotionGenerator(const PxTransform &pose, const PxVec3& linear, const PxVec3& angular)
|
|
: mLinear(linear),
|
|
mAngular(angular),
|
|
mTransform(pose)
|
|
{
|
|
}
|
|
|
|
//----------------------------------------------------------------------------//
|
|
PxVec3 MotionGenerator::getLinearVelocity( float time )
|
|
{
|
|
float t = time - (int(time) & ~0xf);
|
|
const float scale = 0.25f * PxPi;
|
|
|
|
if(t > 0 && t < 2)
|
|
return -scale * sinf(t * 0.5f * PxPi) * mLinear;
|
|
|
|
if(t > 8 && t < 10)
|
|
return +scale * sinf(t * 0.5f * PxPi) * mLinear;
|
|
|
|
return PxVec3(0.0f);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------//
|
|
PxVec3 MotionGenerator::getAngularVelocity( float time )
|
|
{
|
|
float t = time - (int(time) & ~0xf);
|
|
|
|
if(t > 4 && t < 6)
|
|
return +PxPi * mAngular;
|
|
|
|
if(t > 12 && t < 14)
|
|
return -PxPi * mAngular;
|
|
|
|
return PxVec3(0.0f);
|
|
}
|
|
|
|
static PxQuat computeQuatFromAngularVelocity(const PxVec3 &omega)
|
|
{
|
|
PxReal angle = omega.magnitude();
|
|
|
|
if (angle < 1e-5f)
|
|
{
|
|
return PxQuat(PxIdentity);
|
|
} else {
|
|
PxReal s = sin( 0.5f * angle ) / angle;
|
|
PxReal x = omega[0] * s;
|
|
PxReal y = omega[1] * s;
|
|
PxReal z = omega[2] * s;
|
|
PxReal w = cos( 0.5f * angle );
|
|
return PxQuat(x,y,z,w);
|
|
}
|
|
}
|
|
//----------------------------------------------------------------------------//
|
|
const PxTransform& MotionGenerator::update(float time, float dt)
|
|
{
|
|
PxVec3 dw = dt * getAngularVelocity(time);
|
|
PxQuat dq = computeQuatFromAngularVelocity(dw);
|
|
|
|
mTransform.q = (dq * mTransform.q).getNormalized();
|
|
mTransform.p += dt * getLinearVelocity(time);
|
|
|
|
return mTransform;
|
|
}
|
|
|
|
|
|
|
|
|
|
|