diff options
Diffstat (limited to 'source/blender/ikplugin/intern/itasc_plugin.cpp')
| -rw-r--r-- | source/blender/ikplugin/intern/itasc_plugin.cpp | 52 |
1 files changed, 26 insertions, 26 deletions
diff --git a/source/blender/ikplugin/intern/itasc_plugin.cpp b/source/blender/ikplugin/intern/itasc_plugin.cpp index a288f330224..2e437c627a2 100644 --- a/source/blender/ikplugin/intern/itasc_plugin.cpp +++ b/source/blender/ikplugin/intern/itasc_plugin.cpp @@ -424,16 +424,16 @@ static IK_Data* get_ikdata(bPose *pose) } static double EulerAngleFromMatrix(const KDL::Rotation& R, int axis) { - double t = KDL::sqrt(R(0,0)*R(0,0) + R(0,1)*R(0,1)); + double t = KDL::sqrt(R(0, 0)*R(0, 0) + R(0, 1)*R(0, 1)); if (t > 16.0*KDL::epsilon) { - if (axis == 0) return -KDL::atan2(R(1,2), R(2,2)); - else if (axis == 1) return KDL::atan2(-R(0,2), t); - else return -KDL::atan2(R(0,1), R(0,0)); + if (axis == 0) return -KDL::atan2(R(1, 2), R(2, 2)); + else if (axis == 1) return KDL::atan2(-R(0, 2), t); + else return -KDL::atan2(R(0, 1), R(0, 0)); } else { - if (axis == 0) return -KDL::atan2(-R(2,1), R(1,1)); - else if (axis == 1) return KDL::atan2(-R(0,2), t); + if (axis == 0) return -KDL::atan2(-R(2, 1), R(1, 1)); + else if (axis == 1) return KDL::atan2(-R(0, 2), t); else return 0.0f; } } @@ -441,8 +441,8 @@ static double EulerAngleFromMatrix(const KDL::Rotation& R, int axis) static double ComputeTwist(const KDL::Rotation& R) { // qy and qw are the y and w components of the quaternion from R - double qy = R(0,2) - R(2,0); - double qw = R(0,0) + R(1,1) + R(2,2) + 1; + double qy = R(0, 2) - R(2, 0); + double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1; double tau = 2*KDL::atan2(qy, qw); @@ -471,31 +471,31 @@ static void RemoveEulerAngleFromMatrix(KDL::Rotation& R, double angle, int axis) } #if 0 -static void GetEulerXZY(const KDL::Rotation& R, double& X,double& Z,double& Y) +static void GetEulerXZY(const KDL::Rotation& R, double& X, double& Z, double& Y) { - if (fabs(R(0,1)) > 1.0 - KDL::epsilon ) { - X = -KDL::sign(R(0,1)) * KDL::atan2(R(1,2), R(1,0)); - Z = -KDL::sign(R(0,1)) * KDL::PI / 2; + if (fabs(R(0, 1)) > 1.0 - KDL::epsilon ) { + X = -KDL::sign(R(0, 1)) * KDL::atan2(R(1, 2), R(1, 0)); + Z = -KDL::sign(R(0, 1)) * KDL::PI / 2; Y = 0.0; } else { - X = KDL::atan2(R(2,1), R(1,1)); - Z = KDL::atan2(-R(0,1), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,2)))); - Y = KDL::atan2(R(0,2), R(0,0)); + X = KDL::atan2(R(2, 1), R(1, 1)); + Z = KDL::atan2(-R(0, 1), KDL::sqrt( KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 2)))); + Y = KDL::atan2(R(0, 2), R(0, 0)); } } -static void GetEulerXYZ(const KDL::Rotation& R, double& X,double& Y,double& Z) +static void GetEulerXYZ(const KDL::Rotation& R, double& X, double& Y, double& Z) { - if (fabs(R(0,2)) > 1.0 - KDL::epsilon ) { - X = KDL::sign(R(0,2)) * KDL::atan2(-R(1,0), R(1,1)); - Y = KDL::sign(R(0,2)) * KDL::PI / 2; + if (fabs(R(0, 2)) > 1.0 - KDL::epsilon ) { + X = KDL::sign(R(0, 2)) * KDL::atan2(-R(1, 0), R(1, 1)); + Y = KDL::sign(R(0, 2)) * KDL::PI / 2; Z = 0.0; } else { - X = KDL::atan2(-R(1,2), R(2,2)); - Y = KDL::atan2(R(0,2), KDL::sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,1)))); - Z = KDL::atan2(-R(0,1), R(0,0)); + X = KDL::atan2(-R(1, 2), R(2, 2)); + Y = KDL::atan2(R(0, 2), KDL::sqrt( KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 1)))); + Z = KDL::atan2(-R(0, 1), R(0, 0)); } } #endif @@ -804,16 +804,16 @@ static bool joint_callback(const iTaSC::Timestamp& timestamp, iTaSC::ConstraintV if (chan->rotmode > 0) { /* euler rotations (will cause gimble lock, but this can be alleviated a bit with rotation orders) */ - eulO_to_mat3( rmat,chan->eul, chan->rotmode); + eulO_to_mat3( rmat, chan->eul, chan->rotmode); } else if (chan->rotmode == ROT_MODE_AXISANGLE) { /* axis-angle - stored in quaternion data, but not really that great for 3D-changing orientations */ - axis_angle_to_mat3( rmat,&chan->quat[1], chan->quat[0]); + axis_angle_to_mat3( rmat, &chan->quat[1], chan->quat[0]); } else { /* quats are normalised before use to eliminate scaling issues */ normalize_qt(chan->quat); - quat_to_mat3( rmat,chan->quat); + quat_to_mat3(rmat, chan->quat); } KDL::Rotation jointRot( rmat[0][0], rmat[1][0], rmat[2][0], @@ -1388,7 +1388,7 @@ static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan) e_matrix& Wq = arm->getWq(); assert(Wq.cols() == (int)weights.size()); for (int q=0; q<Wq.cols(); q++) - Wq(q,q)=weights[q]; + Wq(q, q)=weights[q]; // get the inverse rest pose frame of the base to compute relative rest pose of end effectors // this is needed to handle the enforce parameter // ikscene->pchan[0] is the root channel of the tree |