Vec3 Constraint::LineOnLineContact::
findForceOnBodyBInG(const State& s) const {
const LineOnLineContactImpl& impl = getImpl();
if (impl.isDisabled(s))
return Vec3(0);
const Transform X_GC = findContactFrameInG(s);
const Vec3 f_C = -getMultipliers(s); // watch sign convention
return X_GC.R()*f_C;
}
/*
// 1-dimensional FFT
//
// API
// int fft(float *x_in, float *x_out, int n, int shift);
// INPUT
// x_in - input signal
// n - number of elements for searching Fourier image
// shift - shift between input elements
// OUTPUT
// x_out - output signal (contains 2n elements in order
Re(x_in[0]), Im(x_in[0]), Re(x_in[1]), Im(x_in[1]) and etc.)
// RESULT
// Error status
*/
int fft(float *x_in, float *x_out, int n, int shift)
{
int n1, n2, res, k1, k2, m1, m2, index, idx;
float alpha, beta, gamma, angle, cosAngle, sinAngle;
float tmpGamma, tmpAlpha, tmpBeta;
float tmpRe, tmpIm, phaseRe, phaseIm;
res = getMultipliers(n, &n1, &n2);
if (res == FFT_OK)
{
fft(x_in, x_out, n1, shift);
fft(x_in, x_out, n2, shift);
}
alpha = (float)(2.0 * PI / ((float)n));
beta = (float)(2.0 * PI / ((float)n1));
gamma = (float)(2.0 * PI / ((float)n2));
for (k1 = 0; k1 < n1; k1++)
{
tmpBeta = beta * k1;
for (k2 = 0; k2 < n2; k2++)
{
idx = shift * (n2 * k1 + k2);
x_out[idx] = 0.0;
x_out[idx + 1] = 0.0;
tmpGamma = gamma * k2;
tmpAlpha = alpha * k2;
for (m1 = 0; m1 < n1; m1++)
{
tmpRe = 0.0;
tmpIm = 0.0;
for (m2 = 0; m2 < n2; m2++)
{
angle = tmpGamma * m2;
index = shift * (n1 * m2 + m1);
cosAngle = cosf(angle);
sinAngle = sinf(angle);
tmpRe += x_in[index] * cosAngle + x_in[index + 1] * sinAngle;
tmpIm += x_in[index + 1] * cosAngle - x_in[index] * sinAngle;
}
angle = tmpAlpha * m1;
cosAngle = cosf(angle);
sinAngle = sinf(angle);
phaseRe = cosAngle * tmpRe + sinAngle * tmpIm;
phaseIm = cosAngle * tmpIm - sinAngle * tmpRe;
angle = tmpBeta * m1;
cosAngle = cosf(angle);
sinAngle = sinf(angle);
x_out[idx] += (cosAngle * phaseRe + sinAngle * phaseIm);
x_out[idx + 1] += (cosAngle * phaseIm - sinAngle * phaseRe);
}
}
}
return FFT_OK;
}
Vec3 Constraint::SphereOnPlaneContact::
findForceOnSphereInG(const State& state) const {
const SphereOnPlaneContactImpl& impl = getImpl();
if (impl.isDisabled(state))
return Vec3(0);
const Rotation& R_FP = impl.getParameters(state).m_X_FP.R();
const MobilizedBody& bodyF =
impl.getMobilizedBodyFromConstrainedBody(impl.m_planeBody_F);
const Rotation& R_GF = bodyF.getBodyRotation(state);
const Rotation R_GP = R_GF * R_FP; // orientation of P frame in G
const Vec3 f_PC = -getMultipliers(state); // watch sign convention
return R_GP * f_PC; // return f_GC
}
