Skip to content
Snippets Groups Projects
Commit 5929a656 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

more typedefs

parent 458a2678
No related branches found
No related tags found
No related merge requests found
......@@ -308,16 +308,20 @@ inline void composeOverState(const MatrixBase<D1>& x,
MatrixSizeCheck<11, 1>::check(d);
MatrixSizeCheck<10, 1>::check(x_plus_d);
Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & x( 0 ) );
Map<const Quaternion<typename D1::Scalar> > q ( & x( 3 ) );
Map<const Matrix<typename D1::Scalar, 3, 1> > v ( & x( 7 ) );
Map<const Matrix<typename D2::Scalar, 3, 1> > dp ( & d( 0 ) );
Map<const Quaternion<typename D2::Scalar> > dq ( & d( 3 ) );
Map<const Matrix<typename D2::Scalar, 3, 1> > dv ( & d( 7 ) );
const typename D2::Scalar& dt = d(10);
Map<Matrix<typename D3::Scalar, 3, 1> > p_plus_d ( & x_plus_d( 0 ) );
Map<Quaternion<typename D3::Scalar> > q_plus_d ( & x_plus_d( 3 ) );
Map<Matrix<typename D3::Scalar, 3, 1> > v_plus_d ( & x_plus_d( 7 ) );
typedef typename D1::Scalar T1;
typedef typename D2::Scalar T2;
typedef typename D3::Scalar T3;
Map<const Matrix<T1, 3, 1> > p ( & x( 0 ) );
Map<const Quaternion<T1> > q ( & x( 3 ) );
Map<const Matrix<T1, 3, 1> > v ( & x( 7 ) );
Map<const Matrix<T2, 3, 1> > dp ( & d( 0 ) );
Map<const Quaternion<T2> > dq ( & d( 3 ) );
Map<const Matrix<T2, 3, 1> > dv ( & d( 7 ) );
const typename D2::Scalar& dt = d(10);
Map<Matrix<T3, 3, 1> > p_plus_d ( & x_plus_d( 0 ) );
Map<Quaternion<T3> > q_plus_d ( & x_plus_d( 3 ) );
Map<Matrix<T3, 3, 1> > v_plus_d ( & x_plus_d( 7 ) );
p_plus_d = p + v*dt + 0.5*gravity()*dt*dt + q*dp;
v_plus_d = v + gravity()*dt + q*dv;
......@@ -363,16 +367,20 @@ inline void betweenStates(const MatrixBase<D1>& x1,
MatrixSizeCheck<10, 1>::check(x2);
MatrixSizeCheck<11, 1>::check(x2_minus_x1);
Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & x1(0) );
Map<const Quaternion<typename D1::Scalar> > q1 ( & x1(3) );
Map<const Matrix<typename D1::Scalar, 3, 1> > v1 ( & x1(7) );
Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & x2(0) );
Map<const Quaternion<typename D2::Scalar> > q2 ( & x2(3) );
Map<const Matrix<typename D2::Scalar, 3, 1> > v2 ( & x2(7) );
Map<Matrix<typename D3::Scalar, 3, 1> > dp ( & x2_minus_x1(0) );
Map<Quaternion<typename D3::Scalar> > dq ( & x2_minus_x1(3) );
Map<Matrix<typename D3::Scalar, 3, 1> > dv ( & x2_minus_x1(7) );
T& Dt = x2_minus_x1(10);
typedef typename D1::Scalar T1;
typedef typename D2::Scalar T2;
typedef typename D3::Scalar T3;
Map<const Matrix<T1, 3, 1> > p1 ( & x1(0) );
Map<const Quaternion<T1> > q1 ( & x1(3) );
Map<const Matrix<T1, 3, 1> > v1 ( & x1(7) );
Map<const Matrix<T2, 3, 1> > p2 ( & x2(0) );
Map<const Quaternion<T2> > q2 ( & x2(3) );
Map<const Matrix<T2, 3, 1> > v2 ( & x2(7) );
Map<Matrix<T3, 3, 1> > dp ( & x2_minus_x1(0) );
Map<Quaternion<T3> > dq ( & x2_minus_x1(3) );
Map<Matrix<T3, 3, 1> > dv ( & x2_minus_x1(7) );
T& Dt = x2_minus_x1(10);
betweenStates(p1, q1, v1, p2, q2, v2, dt, dp, dq, dv, Dt);
}
......@@ -403,10 +411,12 @@ inline void QandPmat(const MatrixBase<D1>& th, MatrixBase<D2>& Q, MatrixBase<D3>
P *= 0.5;
}
else{
Matrix<typename D1::Scalar, 3, 3> Id;
typedef typename D1::Scalar T;
Matrix<T, 3, 3> Id;
Id.setIdentity();
Matrix<typename D1::Scalar, 3, 1> u = th / thn;
Matrix<typename D1::Scalar, 3, 3> ux = skew(u);
Matrix<T, 3, 1> u = th / thn;
Matrix<T, 3, 3> ux = skew(u);
Q = Id
+ ((1 - cos(thn))/thn)*ux
......
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