cv::Mat sg::padKernelFFT( cv::Mat const & input, cv::Size const & paddedSize ) { cv::Mat result = cv::Mat::zeros( paddedSize, input.type() ); // Get the 4 quadrants of the input kernel int hwx = input.cols / 2; int hwy = input.rows / 2; int px = hwx; int py = hwy; int fftW = paddedSize.width; int fftH = paddedSize.height; if( input.cols %2 != 0 ) ++px; if( input.rows % 2 != 0 ) ++py; cv::Mat q0( input, cv::Rect( 0, 0, px, py ) ); cv::Mat q02( result, cv::Rect( fftW - px, fftH - py, px, py ) ); cv::Mat q1( input, cv::Rect( px, 0, hwx, py ) ); cv::Mat q12( result, cv::Rect( 0, fftH - py, hwx, py ) ); cv::Mat q2( input, cv::Rect( 0, py, px, hwy ) ); cv::Mat q22( result, cv::Rect( fftW - px, 0, px, hwy ) ); cv::Mat q3( input, cv::Rect( px, py, hwx, hwy ) ); cv::Mat q32( result, cv::Rect( 0, 0, hwx, hwy ) ); q0.copyTo( q02 ); q1.copyTo( q12 ); q2.copyTo( q22 ); q3.copyTo( q32 ); // if our filter doesn't have real and complex components, make the complex part zeros if( result.channels() < 2 ) { cv::Mat planes[] = { result, cv::Mat::zeros( result.size(), CV_32F ) }; cv::merge( planes, 2, result ); } return result; }
void sg::fftshift( cv::Mat & src ) { // If even we can just pivot around the center pixel // if odd, we pivot around center + (1,1) // quadrants arranged q0 q1 // q2 q3 // q0 is located at (0, 0) with width px, height py // q1 is located at (px, 0) with width hw, height py // q2 is located at (0, py) with width px, height hw // q3 is located at (px, py) with width hw, height hw int hwx = src.cols / 2; int hwy = src.rows / 2; int px = hwx; int py = hwy; if( src.cols %2 != 0 ) ++px; if( src.rows % 2 != 0 ) ++py; cv::Mat q0( src, cv::Rect( 0, 0, px, py ) ); cv::Mat q02( src, cv::Rect( hwx, hwy, px, py ) ); cv::Mat q1( src, cv::Rect( px, 0, hwx, py ) ); cv::Mat q12( src, cv::Rect( 0, hwy, hwx, py ) ); cv::Mat q2( src, cv::Rect( 0, py, px, hwy ) ); cv::Mat q22( src, cv::Rect( hwx, 0, px, hwy ) ); cv::Mat q3( src, cv::Rect( px, py, hwx, hwy ) ); cv::Mat q32( src, cv::Rect( 0, 0, hwx, hwy ) ); // q0 is the biggest segment and might overlap with // everything else so it gets done last, we hold it in tmp cv::Mat tmpq0, tmpq2; q0.copyTo( tmpq0 ); q2.copyTo( tmpq2 ); q3.copyTo( q32 ); q1.copyTo( q12 ); tmpq2.copyTo( q22 ); tmpq0.copyTo( q02 ); }
void solveIK(const AwPoint &startJointPos, const AwPoint &midJointPos, const AwPoint &effectorPos, const AwPoint &handlePos, const AwVector &poleVector, double twistValue, AwQuaternion &qStart, AwQuaternion &qMid) // // This is method that actually computes the IK solution. // { // vector from startJoint to midJoint AwVector vector1 = midJointPos - startJointPos; // vector from midJoint to effector AwVector vector2 = effectorPos - midJointPos; // vector from startJoint to handle AwVector vectorH = handlePos - startJointPos; // vector from startJoint to effector AwVector vectorE = effectorPos - startJointPos; // lengths of those vectors double length1 = vector1.length(); double length2 = vector2.length(); double lengthH = vectorH.length(); // component of the vector1 orthogonal to the vectorE AwVector vectorO = vector1 - vectorE*((vector1*vectorE)/(vectorE*vectorE)); ////////////////////////////////////////////////////////////////// // calculate q12 which solves for the midJoint rotation ////////////////////////////////////////////////////////////////// // angle between vector1 and vector2 double vectorAngle12 = vector1.angle(vector2); // vector orthogonal to vector1 and 2 AwVector vectorCross12 = vector1^vector2; double lengthHsquared = lengthH*lengthH; // angle for arm extension double cos_theta = (lengthHsquared - length1*length1 - length2*length2) /(2*length1*length2); if (cos_theta > 1) cos_theta = 1; else if (cos_theta < -1) cos_theta = -1; double theta = acos(cos_theta); // quaternion for arm extension AwQuaternion q12(theta - vectorAngle12, vectorCross12); ////////////////////////////////////////////////////////////////// // calculate qEH which solves for effector rotating onto the handle ////////////////////////////////////////////////////////////////// // vector2 with quaternion q12 applied vector2 = vector2.rotateBy(q12); // vectorE with quaternion q12 applied vectorE = vector1 + vector2; // quaternion for rotating the effector onto the handle AwQuaternion qEH(vectorE, vectorH); ////////////////////////////////////////////////////////////////// // calculate qNP which solves for the rotate plane ////////////////////////////////////////////////////////////////// // vector1 with quaternion qEH applied vector1 = vector1.rotateBy(qEH); if (vector1.isParallel(vectorH)) // singular case, use orthogonal component instead vector1 = vectorO.rotateBy(qEH); // quaternion for rotate plane AwQuaternion qNP; if (!poleVector.isParallel(vectorH) && (lengthHsquared != 0)) { // component of vector1 orthogonal to vectorH AwVector vectorN = vector1 - vectorH*((vector1*vectorH)/lengthHsquared); // component of pole vector orthogonal to vectorH AwVector vectorP = poleVector - vectorH*((poleVector*vectorH)/lengthHsquared); double dotNP = (vectorN*vectorP)/(vectorN.length()*vectorP.length()); if (absoluteValue(dotNP + 1.0) < kEpsilon) { // singular case, rotate halfway around vectorH AwQuaternion qNP1(kPi, vectorH); qNP = qNP1; } else { AwQuaternion qNP2(vectorN, vectorP); qNP = qNP2; } } ////////////////////////////////////////////////////////////////// // calculate qTwist which adds the twist ////////////////////////////////////////////////////////////////// AwQuaternion qTwist(twistValue, vectorH); // quaternion for the mid joint qMid = q12; // concatenate the quaternions for the start joint qStart = qEH*qNP*qTwist; }
void ik2Bsolver::solveIK(const MPoint &startJointPos, const MPoint &midJointPos, const MPoint &effectorPos, const MPoint &handlePos, const MVector &poleVector, double twistValue, MQuaternion &qStart, MQuaternion &qMid, const double & softDistance, const double & restLength1, const double & restLength2, double & stretching) // // This is method that actually computes the IK solution. // { // vector from startJoint to midJoint MVector vector1 = midJointPos - startJointPos; vector1 = vector1.normal() * restLength1; // vector from midJoint to effector MVector vector2 = effectorPos - midJointPos; vector2 = vector2.normal() * restLength2; // vector from startJoint to handle MVector vectorH = handlePos - startJointPos; // vector from startJoint to effector MVector vectorE = effectorPos - startJointPos; // lengths of those vectors const double length1 = restLength1; const double length2 = restLength2; const double lengthH = vectorH.length(); // component of the vector1 orthogonal to the vectorE const MVector vectorO = vector1 - vectorE*((vector1*vectorE)/(vectorE*vectorE)); ////////////////////////////////////////////////////////////////// // calculate q12 which solves for the midJoint rotation ////////////////////////////////////////////////////////////////// // angle between vector1 and vector2 const double vectorAngle12 = vector1.angle(vector2); // vector orthogonal to vector1 and 2 const MVector vectorCross12 = vector1^vector2; const double lengthHsquared = lengthH * lengthH; double weight = 0.0; double slowLH = lengthH;// / (1.0 + (6.8 - softDistance) / (restLength1 + restLength2)); const double da = restLength1 + restLength2 - softDistance; if(slowLH > da) { float s = (slowLH - da) / softDistance; if(s> 1.f) s= 1.f; Vector3F pt = m_herm.interpolate(s); // MGlobal::displayInfo(MString("herm ")+ pt.y); // approading l1+l2 slower // weight = 1.0 - exp(-(slowLH - da) / softDistance * 6.98); weight = pt.y * weight + (1.f - pt.y) * s; slowLH = da + softDistance * weight; // MGlobal::displayInfo(MString("wei ")+weight); } // // 1 // / \ // l1 / \ l2 // / \ ---l1---1 ---l2--- // 0 ------ l ------ 2 0 ------ l ------- 2 // // angle for arm extension double cos_theta = (slowLH * slowLH - length1*length1 - length2*length2) / (2*length1*length2); if (cos_theta > 1) cos_theta = 1; else if (cos_theta < -1) cos_theta = -1; const double theta = acos(cos_theta); // quaternion for arm extension MQuaternion q12(theta - vectorAngle12, vectorCross12); ////////////////////////////////////////////////////////////////// // calculate qEH which solves for effector rotating onto the handle ////////////////////////////////////////////////////////////////// // vector2 with quaternion q12 applied vector2 = vector2.rotateBy(q12); // vectorE with quaternion q12 applied vectorE = vector1 + vector2; // quaternion for rotating the effector onto the handle MQuaternion qEH(vectorE, vectorH); if(lengthH > vectorE.length()) { // MGlobal::displayInfo(MString("vle ")+(lengthH-vectorE.length())); stretching = (lengthH-vectorE.length()) * weight; } ////////////////////////////////////////////////////////////////// // calculate qNP which solves for the rotate plane ////////////////////////////////////////////////////////////////// // vector1 with quaternion qEH applied vector1 = vector1.rotateBy(qEH); if (vector1.isParallel(vectorH)) // singular case, use orthogonal component instead vector1 = vectorO.rotateBy(qEH); // quaternion for rotate plane MQuaternion qNP; if (!poleVector.isParallel(vectorH) && (lengthHsquared != 0)) { // component of vector1 orthogonal to vectorH MVector vectorN = vector1 - vectorH*((vector1*vectorH)/lengthHsquared); // component of pole vector orthogonal to vectorH MVector vectorP = poleVector - vectorH*((poleVector*vectorH)/lengthHsquared); double dotNP = (vectorN*vectorP)/(vectorN.length()*vectorP.length()); if (absoluteValue(dotNP + 1.0) < kEpsilon) { // singular case, rotate halfway around vectorH MQuaternion qNP1(kPi, vectorH); qNP = qNP1; } else { MQuaternion qNP2(vectorN, vectorP); qNP = qNP2; } } ////////////////////////////////////////////////////////////////// // calculate qTwist which adds the twist ////////////////////////////////////////////////////////////////// MQuaternion qTwist(twistValue, vectorH); // quaternion for the mid joint qMid = q12; // concatenate the quaternions for the start joint qStart = qEH*qNP*qTwist; }