File size: 3,431 Bytes
0c51b93 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 |
#include "../include/NvFlexExt.h"
#include "../core/maths.h"
namespace
{
// internal version of the public NvFlexExtMovingFrame type
// this must be a byte exact copy of of NvFlexExtMovingFrame
// we just separate the internal types to avoid exposing
// our math types on the API
struct MovingFrame
{
Vec3 position;
Quat rotation;
Vec3 velocity;
Vec3 omega;
Vec3 acceleration;
Vec3 tau;
Matrix44 delta;
MovingFrame(Vec3 worldTranslation, Quat worldRotation)
{
position = worldTranslation;
rotation = worldRotation;
delta = Matrix44::kIdentity;
}
// update the frame, returns a matrix representing the delta transform that
// can be applied to particles to teleport them to the frame's new location
void Move(Vec3 newPosition, Quat newRotation, float dt)
{
const float invDt = 1.0f/dt;
// calculate new velocity
Vec3 newVelocity = (newPosition-position)*invDt;
Vec3 newOmega = 2.0f*Vec3(Quat(newRotation-rotation)*Inverse(rotation)*invDt);
// calculate new acceleration
Vec3 newAcceleration = (newVelocity-velocity)*invDt;
Vec3 newTau = (newOmega-omega)*invDt;
// calculate delta transform
Matrix44 LocalFromOld = AffineInverse(TranslationMatrix(Point3(position))*RotationMatrix(rotation));
Matrix44 NewFromLocal = TranslationMatrix(Point3(newPosition))*RotationMatrix(newRotation);
// update delta transform
delta = NewFromLocal*LocalFromOld;
// position
position = newPosition;
rotation = newRotation;
// update velocity
velocity = newVelocity;
omega = newOmega;
acceleration = newAcceleration;
tau = newTau;
}
inline Vec3 GetLinearForce() const
{
return -acceleration;
}
inline Vec3 GetAngularForce(Vec3 p) const
{
Vec3 d = p-position;
// forces due to rotation
Vec3 centrifugalForce = -Cross(omega, Cross(omega, d));
Vec3 eulerForce = -Cross(tau, d);
return centrifugalForce + eulerForce;
}
};
static_assert(sizeof(NvFlexExtMovingFrame) == sizeof(MovingFrame), "Size mismatch for NvFlexExtMovingFrame");
} // anonymous namespace
void NvFlexExtMovingFrameInit(NvFlexExtMovingFrame* frame, const float* worldTranslation, const float* worldRotation)
{
((MovingFrame&)(*frame)) = MovingFrame(Vec3(worldTranslation), Quat(worldRotation));
}
void NvFlexExtMovingFrameUpdate(NvFlexExtMovingFrame* frame, const float* worldTranslation, const float* worldRotation, float dt)
{
((MovingFrame&)(*frame)).Move(Vec3(worldTranslation), Quat(worldRotation), dt);
}
void NvFlexExtMovingFrameApply(NvFlexExtMovingFrame* frame, float* positions, float* velocities, int numParticles, float linearScale, float angularScale, float dt)
{
const MovingFrame& f = (MovingFrame&)(*frame);
// linear force constant for all particles
Vec3 linearForce = f.GetLinearForce()*linearScale;
for (int i=0; i < numParticles; ++i)
{
Vec3 particlePos = Vec3(&positions[i*4]);
Vec3 particleVel = Vec3(&velocities[i*3]);
// angular force depends on particles position
Vec3 angularForce = f.GetAngularForce(particlePos)*angularScale;
// transform particle to frame's new location
particlePos = Vec3(f.delta*Vec4(particlePos, 1.0f));
particleVel += (linearForce + angularForce)*dt;
// update positions
positions[i*4+0] = particlePos.x;
positions[i*4+1] = particlePos.y;
positions[i*4+2] = particlePos.z;
velocities[i*3+0] = particleVel.x;
velocities[i*3+1] = particleVel.y;
velocities[i*3+2] = particleVel.z;
}
}
|