Skip to content
Snippets Groups Projects
Commit 6a18e6a4 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

reimplemented betweenStates and new betweenStatesGlobalAcceleration

parent 75cbd856
No related branches found
No related tags found
1 merge request!32Draft: External forces to force torque inertial preintegration
......@@ -644,22 +644,24 @@ template <typename D1,
typename D16,
typename D17,
typename D18,
typename D19,
class T>
inline void betweenStates(const MatrixBase<D1>& p1,
const MatrixBase<D2>& v1,
const MatrixBase<D5>& L1,
const QuaternionBase<D6>& q1,
const MatrixBase<D7>& p2,
const MatrixBase<D8>& v2,
const MatrixBase<D11>& L2,
const QuaternionBase<D12>& q2,
const T dt,
MatrixBase<D13>& dpi,
MatrixBase<D14>& dvi,
MatrixBase<D15>& dpd,
MatrixBase<D16>& dvd,
MatrixBase<D17>& dL,
QuaternionBase<D18>& dq)
inline void betweenStatesGlobalAcceleration(const MatrixBase<D1>& p1,
const MatrixBase<D2>& v1,
const MatrixBase<D5>& L1,
const QuaternionBase<D6>& q1,
const MatrixBase<D7>& p2,
const MatrixBase<D8>& v2,
const MatrixBase<D11>& L2,
const QuaternionBase<D12>& q2,
const MatrixBase<D13>& g,
const T dt,
MatrixBase<D14>& dpi,
MatrixBase<D15>& dvi,
MatrixBase<D16>& dpd,
MatrixBase<D17>& dvd,
MatrixBase<D18>& dL,
QuaternionBase<D19>& dq)
{
MatrixSizeCheck<3, 1>::check(p1);
MatrixSizeCheck<3, 1>::check(v1);
......@@ -667,25 +669,31 @@ inline void betweenStates(const MatrixBase<D1>& p1,
MatrixSizeCheck<3, 1>::check(p2);
MatrixSizeCheck<3, 1>::check(v2);
MatrixSizeCheck<3, 1>::check(L2);
MatrixSizeCheck<3, 1>::check(g);
MatrixSizeCheck<3, 1>::check(dpi);
MatrixSizeCheck<3, 1>::check(dvi);
MatrixSizeCheck<3, 1>::check(dpd);
MatrixSizeCheck<3, 1>::check(dvd);
MatrixSizeCheck<3, 1>::check(dL);
dpi = q1.conjugate() * (p2 - p1 - v1 * dt - 0.5 * gravity() * dt * dt);
dvi = q1.conjugate() * (v2 - v1 - gravity() * dt);
dpi = q1.conjugate() * (p2 - p1 - v1 * dt - 0.5 * g * dt * dt);
dvi = q1.conjugate() * (v2 - v1 - g * dt);
dpd = dpi;
dvd = dvi;
dL = q1.conjugate() * (L2 - L1);
dq = q1.conjugate() * q2;
}
template <typename D1, typename D2, typename D3, class T>
inline void betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt, MatrixBase<D3>& x2_minus_x1)
template <typename D1, typename D2, typename D3, typename D4, class T>
inline void betweenStatesGlobalAcceleration(const MatrixBase<D1>& x1,
const MatrixBase<D2>& x2,
const MatrixBase<D3>& g,
T dt,
MatrixBase<D4>& x2_minus_x1)
{
MatrixSizeCheck<13, 1>::check(x1);
MatrixSizeCheck<13, 1>::check(x2);
MatrixSizeCheck<3, 1>::check(g);
MatrixSizeCheck<19, 1>::check(x2_minus_x1);
Map<const Matrix<typename D1::Scalar, 3, 1>> p1(&x1(0));
......@@ -703,37 +711,120 @@ inline void betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T
Map<Matrix<typename D3::Scalar, 3, 1>> dL(&x2_minus_x1(12));
Map<Quaternion<typename D3::Scalar>> dq(&x2_minus_x1(15));
betweenStates(p1, v1, L1, q1, p2, v2, L2, q2, dt, dpi, dvi, dpd, dvd, dL, dq);
betweenStatesGlobalAcceleration(p1, v1, L1, q1, p2, v2, L2, q2, g, dt, dpi, dvi, dpd, dvd, dL, dq);
}
template <typename D1, typename D2, class T>
inline Matrix<typename D1::Scalar, 19, 1> betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt)
template <typename D1, typename D2, typename D3, class T>
inline Matrix<typename D1::Scalar, 19, 1> betweenStatesGlobalAcceleration(const MatrixBase<D1>& x1,
const MatrixBase<D2>& x2,
const MatrixBase<D3>& g,
T dt)
{
Matrix<typename D1::Scalar, 19, 1> ret;
betweenStates(x1, x2, dt, ret);
betweenStatesGlobalAcceleration(x1, x2, g, dt, ret);
return ret;
}
inline void betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt, VectorComposite& ret)
inline void betweenStatesGlobalAcceleration(const VectorComposite& x1,
const VectorComposite& x2,
const Vector3d& g,
double dt, VectorComposite& ret)
{
Map<const Quaterniond> q1(x1.at('O').data());
Map<const Quaterniond> q2(x2.at('O').data());
ret['P'] = q1.conjugate() * (x2.at('P') - x1.at('P') - x1.at('V') * dt - 0.5 * gravity() * dt * dt);
ret['V'] = q1.conjugate() * (x2.at('V') - x1.at('V') - gravity() * dt);
ret['p'] = ret.at('P');
ret['v'] = ret.at('V');
ret['L'] = q1.conjugate() * (x2.at('L') - x1.at('L'));
ret['O'] = (q1.conjugate() * q2).coeffs();
Map<Quaterniond> dq(ret.at('O').data());
betweenStatesGlobalAcceleration(x1.at('P'),
x1.at('V'),
x1.at('L'),
q1,
x2.at('P'),
x2.at('V'),
x2.at('L'),
q2,
g,
dt,
ret.at('P'),
ret.at('V'),
ret.at('p'),
ret.at('v'),
ret.at('L'),
dq);
// ret['P'] = q1.conjugate() * (x2.at('P') - x1.at('P') - x1.at('V') * dt - 0.5 * g * dt * dt);
// ret['V'] = q1.conjugate() * (x2.at('V') - x1.at('V') - g * dt);
// ret['p'] = ret.at('P');
// ret['v'] = ret.at('V');
// ret['L'] = q1.conjugate() * (x2.at('L') - x1.at('L'));
// ret['O'] = (q1.conjugate() * q2).coeffs();
}
inline VectorComposite betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt)
inline VectorComposite betweenStatesGlobalAcceleration(const VectorComposite& x1,
const VectorComposite& x2,
const Vector3d& g,
double dt)
{
VectorComposite ret;
betweenStates(x1, x2, dt, ret);
betweenStatesGlobalAcceleration(x1, x2, g, dt, ret);
return ret;
}
template <typename D1,
typename D2,
typename D5,
typename D6,
typename D7,
typename D8,
typename D11,
typename D12,
typename D13,
typename D14,
typename D15,
typename D16,
typename D17,
typename D18,
class T>
inline void betweenStates(const MatrixBase<D1>& p1,
const MatrixBase<D2>& v1,
const MatrixBase<D5>& L1,
const QuaternionBase<D6>& q1,
const MatrixBase<D7>& p2,
const MatrixBase<D8>& v2,
const MatrixBase<D11>& L2,
const QuaternionBase<D12>& q2,
const T dt,
MatrixBase<D13>& dpi,
MatrixBase<D14>& dvi,
MatrixBase<D15>& dpd,
MatrixBase<D16>& dvd,
MatrixBase<D17>& dL,
QuaternionBase<D18>& dq)
{
betweenStatesGlobalAcceleration(p1, v1, L1, q1, p2, v2, L2, q2, gravity(), dt, dpi, dvi, dpd, dvd, dL, dq);
}
template <typename D1, typename D2, typename D3, class T>
inline void betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt, MatrixBase<D3>& x2_minus_x1)
{
betweenStatesGlobalAcceleration(x1, x2, gravity(), dt, x2_minus_x1);
}
template <typename D1, typename D2, class T>
inline Matrix<typename D1::Scalar, 19, 1> betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt)
{
return betweenStatesGlobalAcceleration(x1, x2, gravity(), dt);;
}
inline void betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt, VectorComposite& ret)
{
betweenStatesGlobalAcceleration(x1, x2, gravity(), dt, ret);
}
inline VectorComposite betweenStates(const VectorComposite& x1, const VectorComposite& x2, double dt)
{
return betweenStatesGlobalAcceleration(x1, x2, gravity(), dt);
}
template <typename Derived>
Matrix<typename Derived::Scalar, 18, 1> log_fti(const MatrixBase<Derived>& delta_in)
{
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment