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;
}
