void updateSwarmWithSignedDistance(int distanceID, float fDeltaTime, float sdInfluence)
{
// update the signed distance function based on the time passed.
updateSD(distanceID, fDeltaTime);
for (int i = 0; i < NUM_TRIANGLES; i++)
{
// update direction based on destination point
float destDirection[3];
destDirection[0] = tris.moveToPoint[i][0] - tris.position[i][0];
destDirection[1] = tris.moveToPoint[i][1] - tris.position[i][1];
destDirection[2] = tris.moveToPoint[i][2] - tris.position[i][2];
// normalize the length to where we go so that the speed is fixed.
normalize(destDirection, 3);
slerp(tris.direction[i], destDirection, tris.direction[i], 3, 1.8f * fDeltaTime);
reNormal(tris.normal[i], tris.direction[i], 3);
// update normal based on signed distance
float signedDist = getSD(tris.position[i], distanceID);
getSDNormal(tris.position[i], destDirection, distanceID);
float t = 1.0f - fabsf(signedDist);
t = t < 0.0f ? 0.0f : t;
t = t > 1.0f ? 1.0f : t;
slerp(tris.normal[i], destDirection, tris.normal[i], 3, t * sdInfluence);
reNormal(tris.direction[i], tris.normal[i], 3);
}
}
Quat_<Real> Quat_<Real>::baryCentric(Real f, Real g, const Quat_& q0, const Quat_& q1, const Quat_& q2)
{
f = Alge::clamp(f, 0, 1);
g = Alge::clamp(g, 0, 1);
Real t = f+g;
return (t != 0) ? slerp(g/t, slerp(t, q0, q1), slerp(t, q0, q2)) : q0;
}
// -----------------------------------------------------------------------------------------
CPepeEngineQuaternion CPepeEngineQuaternion::squad(
float fT, const CPepeEngineQuaternion& rkP, const CPepeEngineQuaternion& rkA,
const CPepeEngineQuaternion& rkB, const CPepeEngineQuaternion& rkQ, bool shortestPath)
{
float fSlerpT = 2.0f * fT * (1.0f - fT);
CPepeEngineQuaternion kSlerpP = slerp(fT, rkP, rkQ, shortestPath);
CPepeEngineQuaternion kSlerpQ = slerp(fT, rkA, rkB);
return slerp(fSlerpT, kSlerpP , kSlerpQ);
}
void akPoseBlender::blendJoint(BlendMode bmode, RotMode rmode, akScalar weight, const akTransformState& a, const akTransformState& b, akTransformState& out)
{
switch(bmode)
{
case PB_BM_LERP:
out.loc = lerp(weight, a.loc, b.loc);
if(rmode==PB_RM_SLERP)
out.rot = slerp(weight, a.rot, b.rot);
else
out.rot = lerp(weight, a.rot, b.rot);
out.scale = lerp(weight, a.scale, b.scale);
break;
case PB_BM_ADD:
out.loc = a.loc + b.loc * weight;
if(rmode==PB_RM_SLERP)
out.rot = slerp(weight, a.rot, a.rot*b.rot);
else
out.rot = lerp(weight, a.rot, a.rot*b.rot);
out.scale = lerp(weight, a.scale, mulPerElem(a.scale, b.scale) );
// {
// akTransformState sum;
// akMathUtils::extractTransform(a.toMatrix()*b.toMatrix(), sum.loc, sum.rot, sum.scale);
// out.loc = lerp(weight, a.loc, sum.loc);
// if(rmode==PB_RM_SLERP)
// out.rot = slerp(weight, a.rot, sum.rot);
// else
// out.rot = lerp(weight, a.rot, sum.rot);
// out.scale = lerp(weight, a.scale, sum.scale);
// }
break;
//TODO test this.
case PB_BM_SUB:
out.loc = a.loc - b.loc * weight;
akQuat invrot = conj(b.rot);
if(rmode==PB_RM_SLERP)
out.rot = slerp(weight, a.rot, a.rot*invrot);
else
out.rot = lerp(weight, a.rot, a.rot*invrot);
// Needs a check for dision by 0 (not likely to happen).
out.scale = lerp(weight, a.scale, divPerElem(a.scale, b.scale) );
break;
}
}
void
TimeCache::interpolate(const TransformStorage& one,
const TransformStorage& two,
fawkes::Time time, TransformStorage& output)
{
// Check for zero distance case
if( two.stamp == one.stamp ) {
output = two;
return;
}
//Calculate the ratio
btScalar ratio =
(time.in_sec() - one.stamp.in_sec()) /
(two.stamp.in_sec() - one.stamp.in_sec());
//Interpolate translation
output.translation.setInterpolate3(one.translation, two.translation, ratio);
//Interpolate rotation
output.rotation = slerp( one.rotation, two.rotation, ratio);
output.stamp = one.stamp;
output.frame_id = one.frame_id;
output.child_frame_id = one.child_frame_id;
}
void BoneNode::calcFrameTransform( Matrix4& result , float* frame , float* weights , int num )
{
assert( keyFrames );
assert( num != 0 );
int curFrame = (int)frame[0];
Quaternion accRotation;
Vector3 accPos;
interpolationMotion( accPos , accRotation , curFrame , frame[0] - curFrame );
float accWeight = weights[0];
for( int i = 1 ; i < num ; ++i )
{
Quaternion rotate;
Vector3 pos;
curFrame = (int)frame[i];
interpolationMotion( pos , rotate , curFrame , frame[i] - curFrame );
accWeight += weights[i];
float t = weights[i] / accWeight;
accRotation = slerp( accRotation , rotate , t );
accPos = lerp( accPos , pos , t );
}
result.setTransform( accPos , accRotation );
}
void TransformationMatrix::blend(const TransformationMatrix& from, double progress)
{
if (from.isIdentity() && isIdentity())
return;
// decompose
DecomposedType fromDecomp;
DecomposedType toDecomp;
from.decompose(fromDecomp);
decompose(toDecomp);
// interpolate
blendFloat(fromDecomp.scaleX, toDecomp.scaleX, progress);
blendFloat(fromDecomp.scaleY, toDecomp.scaleY, progress);
blendFloat(fromDecomp.scaleZ, toDecomp.scaleZ, progress);
blendFloat(fromDecomp.skewXY, toDecomp.skewXY, progress);
blendFloat(fromDecomp.skewXZ, toDecomp.skewXZ, progress);
blendFloat(fromDecomp.skewYZ, toDecomp.skewYZ, progress);
blendFloat(fromDecomp.translateX, toDecomp.translateX, progress);
blendFloat(fromDecomp.translateY, toDecomp.translateY, progress);
blendFloat(fromDecomp.translateZ, toDecomp.translateZ, progress);
blendFloat(fromDecomp.perspectiveX, toDecomp.perspectiveX, progress);
blendFloat(fromDecomp.perspectiveY, toDecomp.perspectiveY, progress);
blendFloat(fromDecomp.perspectiveZ, toDecomp.perspectiveZ, progress);
blendFloat(fromDecomp.perspectiveW, toDecomp.perspectiveW, progress);
slerp(&fromDecomp.quaternionX, &toDecomp.quaternionX, progress);
// recompose
recompose(fromDecomp);
}
void RunQuaternionTests()
{
RunCommonVectorOrQuaternionTests<quaternion>("quaternion");
RunPerfTest<quaternion, quaternion>("quaternion conjugate", [](quaternion* value, quaternion& param)
{
auto t = param;
param = *value;
*value = conjugate(t);
});
RunPerfTest<quaternion, quaternion>("quaternion inverse", [](quaternion* value, quaternion& param)
{
auto t = param;
param = *value;
*value = inverse(t);
});
RunPerfTest<quaternion, quaternion>("quaternion slerp", [](quaternion* value, quaternion const& param)
{
*value = slerp(*value, param, 0.5f);
});
RunPerfTest<quaternion, quaternion>("quaternion concatenate", [](quaternion* value, quaternion const& param)
{
*value = concatenate(*value, param);
});
}
quat quat::squad(const quat& q1, const quat& q2, const quat& q3, float t, bool shortestPath) const
{
float slerpT(2.0f * t * (1.0f - t));
quat slerpP(slerp(q3, t, shortestPath));
quat slerpQ(q1.slerp(q2, t, false));
return slerpP.slerp(slerpQ, slerpT, shortestPath);
}
void inter_dcgan(char *cfgfile, char *weightfile)
{
network *net = load_network(cfgfile, weightfile, 0);
set_batch_network(net, 1);
srand(2222222);
clock_t time;
char buff[256];
char *input = buff;
int i, imlayer = 0;
for (i = 0; i < net->n; ++i) {
if (net->layers[i].out_c == 3) {
imlayer = i;
printf("%d\n", i);
break;
}
}
image start = random_unit_vector_image(net->w, net->h, net->c);
image end = random_unit_vector_image(net->w, net->h, net->c);
image im = make_image(net->w, net->h, net->c);
image orig = copy_image(start);
int c = 0;
int count = 0;
int max_count = 15;
while(1){
++c;
if(count == max_count){
count = 0;
free_image(start);
start = end;
end = random_unit_vector_image(net->w, net->h, net->c);
if(c > 300){
end = orig;
}
if(c>300 + max_count) return;
}
++count;
slerp(start.data, end.data, (float)count / max_count, im.w*im.h*im.c, im.data);
float *X = im.data;
time=clock();
network_predict(net, X);
image out = get_network_image_layer(net, imlayer);
//yuv_to_rgb(out);
normalize_image(out);
printf("%s: Predicted in %f seconds.\n", input, sec(clock()-time));
//char buff[256];
sprintf(buff, "out%05d", c);
save_image(out, "out");
save_image(out, buff);
show_image(out, "out", 0);
}
}
void AnimationTrack::linearDeformation()
{
Vec3 trans = slerp(this->interpolationBeginKeyFrame->getTranslation(),
this->interpolationEndKeyFrame->getTranslation(), this->frameProportion);
Quat rotate = slerp(this->interpolationBeginKeyFrame->getRotation(),
this->interpolationEndKeyFrame->getRotation(), this->frameProportion);
Vec3 scale = slerp(this->interpolationBeginKeyFrame->getScaling(),
this->interpolationEndKeyFrame->getScaling(), this->frameProportion);
auto entity = this->entity.lock();
TransformPtr& transform = entity->getTransform();
transform->setLocalTranslation(trans);
transform->setLocalRotation(rotate);
transform->setLocalScaling(scale);
}
Quaternion XFormNode::get_rotation(long time) const
{
Animation *anim = anims[curr_anim];
/*if the <vector> is empty return an identity quaternion.*/
if(anim->rotation_is_empty()){
return Quaternion();
}
KeyframeQuat first_keyframe = anim->get_rotation_keyframe(0);
KeyframeQuat last_keyframe = anim->get_rotation_keyframe(anim->get_rotation_count() - 1);
long start_time=first_keyframe.time; //start time of the animation.
long full_interval=last_keyframe.time-start_time;//full duration of the animation.
//animation loop algorithm----------
if(full_interval && anim->is_loop_enabled()){
time -= start_time;
time %= full_interval;
time += start_time;
}
//----------------------------------
if(time < first_keyframe.time){
return first_keyframe.r;
}
if(time >= last_keyframe.time){
return last_keyframe.r;
}
long interval;
double t;
for(unsigned int i = 0 ; i < anim->get_rotation_count() - 1 ; i++){
KeyframeQuat current = anim->get_rotation_keyframe(i);
KeyframeQuat next = anim->get_rotation_keyframe(i + 1);
if( (time >= current.time) && (time <= next.time) ){
Quaternion tmp_rot;
/*calculation of the interval between keyframes.*/
interval = next.time - current.time;
/*calculation of the interpolated parameter t.*/
t = (double)(time - current.time) / (double)interval;
/*Spherical linear interpolation--->>>SLERP!(cause we have rotations).*/
tmp_rot = slerp(current.r , next.r , t);
return tmp_rot;
}
}
std::cout<<"Error: Rotation Keyframe not found!!!"<<std::endl;
return Quaternion();
}
Physics::State Physics::interpolate(const State &a, const State &b, float alpha)
{
State state = b;
state._position = a._position*(1-alpha) + b._position*alpha; //alpha needs to be checked for errors. it is currently doing 1-1 no matter what.
state._momentum = a._momentum*(1-alpha) + b._momentum*alpha;
state._orientation = slerp(a._orientation, b._orientation, alpha);
state._angularMomentum = a._angularMomentum*(1-alpha) + b._angularMomentum*alpha;
state.recalculate();
return state;
}
// lerp whenever possible
LLQuaternion nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b)
{
if (dot(a, b) < 0.f)
{
return slerp(t, a, b);
}
else
{
return lerp(t, a, b);
}
}
LLQuaternion nlerp(F32 t, const LLQuaternion &q)
{
if (q.mQ[VW] < 0.f)
{
return slerp(t, q);
}
else
{
return lerp(t, q);
}
}
const Transform2 slerp(const Transform2& a, const Transform2& b, const float t)
{
GEOMETRY_RUNTIME_ASSERT(a.scaling > 0.0f);
GEOMETRY_RUNTIME_ASSERT(b.scaling > 0.0f);
return Transform2(
mix(a.translation, b.translation, t),
slerp(a.rotation, b.rotation, t),
Math::mix(a.scaling, b.scaling, t)
);
}
void BoneNode::calcFrameTransform( Matrix4& result , int frame0 , int frame1 , float t )
{
assert( keyFrames );
MotionKeyFrame& keyFrame0 = keyFrames[ frame0 ];
MotionKeyFrame& keyFrame1 = keyFrames[ frame1 ];
Quaternion q = slerp( keyFrame0.rotation , keyFrame1.rotation , t );
Vector3 pos = lerp( keyFrame0.pos , keyFrame1.pos , t );
result.setTransform( pos , q );
}
//-----------------------------------------------------------------------------
// LLKeyframeFallMotion::onUpdate()
//-----------------------------------------------------------------------------
BOOL LLKeyframeFallMotion::onUpdate(F32 activeTime, U8* joint_mask)
{
BOOL result = LLKeyframeMotion::onUpdate(activeTime, joint_mask);
F32 slerp_amt = clamp_rescale(activeTime / getDuration(), 0.5f, 0.75f, 0.f, 1.f);
if (mPelvisState.notNull())
{
mPelvisState->setRotation(mPelvisState->getRotation() * slerp(slerp_amt, mRotationToGroundNormal, LLQuaternion()));
}
return result;
}
void MD5Model::animate(float dt) {
// sanity check #1
if (currAnim < 0 || currAnim >= int(anims.size()) || !anims[currAnim])
throw Exception("MD5Model::animate(): currAnim is invalid");
Anim *anim = anims[currAnim];
// sanity check #2
if (currFrame < 0 || currFrame >= int(anim->numFrames))
throw Exception("MD5Model::animate(): currFrame is invalid");
// compute index of next frame
int nextFrameIndex = currFrame >= anim->numFrames - 1 ? 0 : currFrame + 1;
// update animation time
animTime += dt*float(anim->frameRate);
if (animTime > 1.0f) {
while (animTime > 1.0f)
animTime -= 1.0f;
//setFrame(nextFrameIndex);
currFrame = nextFrameIndex;
nextFrameIndex = currFrame >= anim->numFrames - 1 ? 0 : currFrame + 1;
}
// make sure size of storage for interpolated frame is correct
if (int(interpFrame.joints.size()) != numJoints)
interpFrame.joints.resize(numJoints);
///// now interpolate between the two frames /////
Frame &frame = anim->frames[currFrame],
&nextFrame = anim->frames[nextFrameIndex];
// interpolate between the joints of the current frame and those of the next
// frame and store them in interpFrame
for (int i = 0; i < numJoints; i++) {
Joint &interpJoint = interpFrame.joints[i];
// linearly interpolate between joint positions
float *pos1 = frame.joints[i].pos,
*pos2 = nextFrame.joints[i].pos;
interpJoint.pos[0] = pos1[0] + animTime*(pos2[0] - pos1[0]);
interpJoint.pos[1] = pos1[1] + animTime*(pos2[1] - pos1[1]);
interpJoint.pos[2] = pos1[2] + animTime*(pos2[2] - pos1[2]);
interpJoint.quat = slerp(frame.joints[i].quat, nextFrame.joints[i].quat, animTime);
}
buildVerts(interpFrame);
buildNormals();
calculateRenderData();
}
//-------------------------------------------------------------------------------------
BOOL LLFollowCam::updateBehindnessConstraint(LLVector3 focus, LLVector3& cam_position)
{
BOOL constraint_active = FALSE;
// only apply this stuff if the behindness angle is something other than opened up all the way
if ( mBehindnessMaxAngle < FOLLOW_CAM_MAX_BEHINDNESS_ANGLE - FOLLOW_CAM_BEHINDNESS_EPSILON )
{
//--------------------------------------------------------------
// horizontalized vector from focus to camera
//--------------------------------------------------------------
LLVector3 horizontalVectorFromFocusToCamera;
horizontalVectorFromFocusToCamera.setVec(cam_position - focus);
horizontalVectorFromFocusToCamera.mV[ VZ ] = 0.0f;
F32 cameraZ = cam_position.mV[ VZ ];
//--------------------------------------------------------------
// distance of horizontalized vector
//--------------------------------------------------------------
F32 horizontalDistance = horizontalVectorFromFocusToCamera.magVec();
//--------------------------------------------------------------------------------------------------
// calculate horizontalized back vector of the subject and scale by horizontalDistance
//--------------------------------------------------------------------------------------------------
LLVector3 horizontalSubjectBack( -1.0f, 0.0f, 0.0f );
horizontalSubjectBack *= mSubjectRotation;
horizontalSubjectBack.mV[ VZ ] = 0.0f;
horizontalSubjectBack.normVec(); // because horizontalizing might make it shorter than 1
horizontalSubjectBack *= horizontalDistance;
//--------------------------------------------------------------------------------------------------
// find the angle (in degrees) between these vectors
//--------------------------------------------------------------------------------------------------
F32 cameraOffsetAngle = 0.f;
LLVector3 cameraOffsetRotationAxis;
LLQuaternion camera_offset_rotation;
camera_offset_rotation.shortestArc(horizontalSubjectBack, horizontalVectorFromFocusToCamera);
camera_offset_rotation.getAngleAxis(&cameraOffsetAngle, cameraOffsetRotationAxis);
cameraOffsetAngle *= RAD_TO_DEG;
if ( cameraOffsetAngle > mBehindnessMaxAngle )
{
F32 fraction = ((cameraOffsetAngle - mBehindnessMaxAngle) / cameraOffsetAngle) * LLCriticalDamp::getInterpolant(mBehindnessLag);
cam_position = focus + horizontalSubjectBack * (slerp(fraction, camera_offset_rotation, LLQuaternion::DEFAULT));
cam_position.mV[VZ] = cameraZ; // clamp z value back to what it was before we started messing with it
constraint_active = TRUE;
}
}
return constraint_active;
}
TEST_F(QuaternionTest, sperical_linear_interpolation_makes_correct_quaternion)
{
auto from = create_random_quaternion();
auto to = create_random_quaternion();
quaternion_normalize(from);
quaternion_normalize(to);
const auto t =(rand() % 400) / 400.0;
const auto res = quaternion_slerp(from, to, t);
const auto correct = slerp(from, to, t);
EXPECT_EQ(correct.w(), res.w());
EXPECT_EQ(correct.x(), res.x());
EXPECT_EQ(correct.y(), res.y());
EXPECT_EQ(correct.z(), res.z());
}
void Magic3D::Object::lookAt(Vector3 position, Vector3 up, float factor)
{
Matrix4 m4EyeFrame;
Vector3 v3X, v3Y, v3Z;
Vector3 eyePos = getPositionFromParent();
v3Y = normalize( up );
v3Z = normalize( ( position - eyePos ) );
v3X = normalize( cross( v3Y, v3Z ) );
v3Y = cross( v3Z, v3X );
m4EyeFrame = Matrix4( Vector4( v3X ), Vector4( v3Y ), Vector4( v3Z ), Vector4( eyePos ) );
Quaternion parent = getParent() ? Math::inverse(getParent()->getRotationFromParent()) : Quaternion::identity();
Quaternion m = slerp(factor, getRotationFromParent(), Quaternion(m4EyeFrame.getUpper3x3()));
setRotation(parent * m);
}
void BoneNode::interpolationMotion( Vector3& pos , Quaternion& rotate , int frame , float t )
{
assert( keyFrames );
MotionKeyFrame& frame0 = keyFrames[ frame ];
MotionKeyFrame& frame1 = keyFrames[ frame + 1 ];
if ( t == 0 )
{
pos = frame0.pos;
rotate = frame0.rotation ;
}
else
{
pos = lerp( frame0.pos , frame1.pos , t );
rotate = slerp( frame0.rotation , frame1.rotation , t );
}
}
Foam::tmp<Foam::pointField> Foam::sixDoFRigidBodyMotion::transform
(
const pointField& initialPoints,
const scalarField& scale
) const
{
// Calculate the transformation septerion from the initial state
septernion s
(
centreOfRotation() - initialCentreOfRotation(),
quaternion(Q().T() & initialQ())
);
tmp<pointField> tpoints(new pointField(initialPoints));
pointField& points = tpoints.ref();
forAll(points, pointi)
{
// Move non-stationary points
if (scale[pointi] > SMALL)
{
// Use solid-body motion where scale = 1
if (scale[pointi] > 1 - SMALL)
{
points[pointi] = transform(initialPoints[pointi]);
}
// Slerp septernion interpolation
else
{
septernion ss(slerp(septernion::I, s, scale[pointi]));
points[pointi] =
initialCentreOfRotation()
+ ss.invTransformPoint
(
initialPoints[pointi]
- initialCentreOfRotation()
);
}
}
}
return tpoints;
}
/**
* Animate the cube rotations.
* @param elapsed The elapsed time since the last draw call.
*/
mat4 Cubie::animateCubeRotation(double elapsed)
{
float cosTheta;
// Spherical linear interpolation (SLERP) between the current and desired
// orientations. The orientation needs to be normalized or precision
// will be lost over time, causing constantly animated cubes.
this->cubeRot.orientation = normalize(slerp(this->cubeRot.orientation,
this->cubeRot.desired, this->cubeRot.speed * (float)elapsed));
// The cosine between the orientation and desired.
cosTheta = dot(this->cubeRot.orientation, this->cubeRot.desired);
//if (cosTheta > .999 && cosTheta < 1.001 || cosTheta)
if (fabs(1 - fabs(cosTheta)) < .00001)
this->cubeRot.orientation = this->cubeRot.desired;
return mat4_cast(this->cubeRot.orientation);
}
bool dev::sixense::process_tick(app::event *E)
{
const double dt = E->data.tick.dt;
if (::host->root())
translate();
if (flying)
{
quat o = ::host->get_orientation();
quat q = normal(inverse(init_q) * curr_q);
quat r = normal(slerp(quat(), q, 1.0 / 30.0));
vec3 d = (curr_p - init_p) * dt * move_rate;
::host->set_orientation(o * r);
::host->offset_position(o * d);
}
return false;
}
int main( void ) {
// init
time_t seed = time(NULL);
printf("seed: %ld\n", seed);
srand((unsigned int)seed); // might break in 2038
aa_test_ulimit();
for( size_t i = 0; i < 1000; i++ ) {
/* Random Data */
static const size_t k=2;
double E[2][7], S[2][8], T[2][12], dx[2][6];
for( size_t j = 0; j < k; j ++ ) {
rand_tf(E[j], S[j], T[j]);
aa_vrand(6,dx[j]);
}
//printf("%d\n",i);
/* Run Tests */
rotvec(E[0]);
euler(dx[0]);
euler1(dx[0]);
eulerzyx(E[0]);
chain(E,S,T);
quat(E);
duqu();
rel_q();
rel_d();
slerp();
theta2quat();
rotmat(E[0]);
tfmat();
tfmat_inv(T[0]);
mzlook(dx[0]+0, dx[0]+3, dx[1]+0);
integrate(E[0], S[0], T[0], dx[0]);
tf_conj(E, S);
qdiff(E,dx);
}
return 0;
}
TEST(Bullet, Slerp)
{
unsigned int runs = 100;
seed_rand();
btQuaternion q1, q2;
q1.setEuler(0,0,0);
for (unsigned int i = 0 ; i < runs ; i++)
{
q2.setEuler(1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX,
1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX,
1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX);
btQuaternion q3 = slerp(q1,q2,0.5);
EXPECT_NEAR(q3.angle(q1), q2.angle(q3), 1e-5);
}
}
void Quaternion::slerp(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst)
{
GP_ASSERT(dst);
slerp(q1.x, q1.y, q1.z, q1.w, q2.x, q2.y, q2.z, q2.w, t, &dst->x, &dst->y, &dst->z, &dst->w);
}
void n_tentacle::computeSlerp(const MMatrix &matrix1, const MMatrix &matrix2, const MFnNurbsCurve &curve, double parameter, double blendRot, double iniLength, double curveLength, double stretch, double globalScale, int tangentAxis, MVector &outPos, MVector &outRot)
{
//curveLength = curve.length()
double lenRatio = iniLength / curveLength;
MQuaternion quat1;
quat1 = matrix1;
MQuaternion quat2;
quat2 = matrix2;
this->bipolarityCheck(quat1, quat2);
//need to adjust the parameter in order to maintain the length between elements, also for the stretch
MVector tangent;
MPoint pointAtParam;
MPoint finaPos;
double p = lenRatio * parameter * globalScale;
double finalParam = p + (parameter - p) * stretch;
if(curveLength * finalParam > curveLength)
{
double lengthDiff = curveLength - (iniLength * parameter);
double param = curve.knot(curve.numKnots() - 1);
tangent = curve.tangent(param, MSpace::kWorld);
tangent.normalize();
curve.getPointAtParam(param, pointAtParam, MSpace::kWorld);
finaPos = pointAtParam;
pointAtParam += (- tangent) * lengthDiff;
//MGlobal::displayInfo("sdf");
}
else
{
double param = curve.findParamFromLength(curveLength * finalParam);
tangent = curve.tangent(param, MSpace::kWorld);
tangent.normalize();
curve.getPointAtParam(param, pointAtParam, MSpace::kWorld);
}
MQuaternion slerpQuat = slerp(quat1, quat2, blendRot);
MMatrix slerpMatrix = slerpQuat.asMatrix();
int axisId = abs(tangentAxis) - 1;
MVector slerpMatrixYAxis = MVector(slerpMatrix(axisId, 0), slerpMatrix(axisId, 1), slerpMatrix(axisId, 2));
slerpMatrixYAxis.normalize();
if(tangentAxis < 0)
slerpMatrixYAxis = - slerpMatrixYAxis;
double angle = tangent.angle(slerpMatrixYAxis);
MVector axis = slerpMatrixYAxis ^ tangent;
axis.normalize();
MQuaternion rotationToSnapOnCurve(angle, axis);
MQuaternion finalQuat = slerpQuat * rotationToSnapOnCurve;
MEulerRotation finalEuler = finalQuat.asEulerRotation();
outRot.x = finalEuler.x;
outRot.y = finalEuler.y;
outRot.z = finalEuler.z;
outPos = pointAtParam;
}
