/*!
Compute the skew symmetric matrix \f$M\f$ of translation vector
\f$t\f$ (matrice de pre-produit vectoriel).
\f[ \mbox{if} \quad {\bf t} = \left( \begin{array}{c} t_x \\ t_y \\ t_z
\end{array}\right), \quad \mbox{then} \qquad
M = \left( \begin{array}{ccc}
0 & -t_z & t_y \\
t_z & 0 & -t_x \\
-t_y & t_x & 0
\end{array}\right)
\f]
\param t : Translation vector in input.
\return Skew symmetric matrix \f$M\f$ of translation vector
\f$t\f$
*/
vpMatrix
vpTranslationVector::skew(const vpTranslationVector &t)
{
vpMatrix M(3, 3);
skew(t,M);
return M;
}
inline Matrix<double,3,6> d_expy_d_y(const Vector3d & y){
Matrix<double,3,6> J;
J.topLeftCorner<3,3>() = -skew(y);
J.bottomRightCorner<3,3>().setIdentity();
return J;
}
Vector6d logmap_se3(Matrix4d T){
Matrix3d R, Id3 = Matrix3d::Identity();
Vector3d Vt, t, w;
Matrix3d V = Matrix3d::Identity(), w_hat = Matrix3d::Zero();
Vector6d x;
Vt << T(0,3), T(1,3), T(2,3);
w << 0.f, 0.f, 0.f;
R = T.block(0,0,3,3);
double cosine = (R.trace() - 1.f)/2.f;
if(cosine > 1.f)
cosine = 1.f;
else if (cosine < -1.f)
cosine = -1.f;
double sine = sqrt(1.0-cosine*cosine);
if(sine > 1.f)
sine = 1.f;
else if (sine < -1.f)
sine = -1.f;
double theta = acos(cosine);
if( theta > 0.000001 ){
w_hat = theta*(R-R.transpose())/(2.f*sine);
w = skewcoords(w_hat);
Matrix3d s;
s = skew(w) / theta;
V = Id3 + s * (1.f-cosine) / theta + s * s * (theta - sine) / theta;
}
t = V.inverse() * Vt;
x.head(3) = t;
x.tail(3) = w;
return x;
}
inline Eigen::Matrix<typename D::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(D)::Options>
skew(const Eigen::MatrixBase<D> & v)
{
Eigen::Matrix<typename D::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(D)::Options> M;
skew(v,M);
return M;
}
Twist operator/(const Wrench& h, const RigidBodyInertia& I)
{
//For mathematical details, check:
//Featherstone Book (RBDA, 2008) formuma 2.74, page 36
double m = I.getMass();
Vector com_kdl = I.getCOG();
RotationalInertia Irot = I.getRotationalInertia();
if( m == 0 ) { return Twist::Zero(); }
Eigen::Matrix3d Ic, Ic_inverse, skew_com;
Eigen::Vector3d com;
com = Eigen::Map<Eigen::Vector3d>(com_kdl.data);
skew_com = skew(com);
Ic = Eigen::Map<Eigen::Matrix3d>(Irot.data) + m*skew_com*skew_com;
Ic_inverse = Ic.inverse();
Twist return_value;
Eigen::Map<Eigen::Vector3d>(return_value.vel.data) = ((1/m)*Eigen::Matrix3d::Identity() - skew_com*Ic_inverse*skew_com) * Eigen::Map<const Eigen::Vector3d>(h.force.data) +
skew_com*Ic_inverse*Eigen::Map<const Eigen::Vector3d>(h.torque.data);
Eigen::Map<Eigen::Vector3d>(return_value.rot.data) = (-Ic_inverse*skew_com)*Eigen::Map<const Eigen::Vector3d>(h.force.data)
+ Ic_inverse*Eigen::Map<const Eigen::Vector3d>(h.torque.data);
return return_value;
}
/**
* @short AA tree insert
*
* Returns the root node of the subtree
*/
static onion_dict_node *onion_dict_node_add(onion_dict *d, onion_dict_node *node, onion_dict_node *nnode){
if (node==NULL){
//ONION_DEBUG("Add here %p",nnode);
return nnode;
}
signed int cmp=d->cmp(nnode->data.key, node->data.key);
//ONION_DEBUG0("cmp %d, %X, %X %X",cmp, nnode->data.flags,nnode->data.flags&OD_REPLACE, OD_REPLACE);
if ((cmp==0) && (nnode->data.flags&OD_REPLACE)){
//ONION_DEBUG("Replace %s with %s", node->data.key, nnode->data.key);
onion_dict_node_data_free(&node->data);
memcpy(&node->data, &nnode->data, sizeof(onion_dict_node_data));
free(nnode);
return node;
}
else if (cmp<0){
node->left=onion_dict_node_add(d, node->left, nnode);
//ONION_DEBUG("%p[%s]->left=%p[%s]",node, node->data.key, node->left, node->left->data.key);
}
else{ // >=
node->right=onion_dict_node_add(d, node->right, nnode);
//ONION_DEBUG("%p[%s]->right=%p[%s]",node, node->data.key, node->right, node->right->data.key);
}
node=skew(node);
node=split(node);
return node;
}
/*!
Compute the skew symmetric matrix \f$M\f$ of the translation vector (matrice
de pre-produit vectoriel), where
\f[ M = \left( \begin{array}{ccc}
0 & -t_z & t_y \\
t_z & 0 & -t_x \\
-t_y & t_x & 0
\end{array}\right)
\f]
and where \f$(t_x,t_y,t_z)\f$ are the coordinates of the translation
vector.
\return Skew symmetric matrix \f$M\f$ of the translation vector.
*/
vpMatrix
vpTranslationVector::skew() const
{
vpMatrix M(3, 3);
skew(*this,M);
return M;
}
node_t *cgc_tree_insert(node_t **T, node_t *n)
{
node_t *old = NULL;
node_t *t = *T;
if (t == NULL)
{
*T = n;
n->left = NULL;
n->right = NULL;
n->level = 1;
}
else if (tree_cmp(n->key, n->key_length, t) < 0)
{
old = cgc_tree_insert(&t->left, n);
}
else if (tree_cmp(n->key, n->key_length, t) > 0)
{
old = cgc_tree_insert(&t->right, n);
}
else
{
// replace the node with the new node
old = t;
*T = n;
n->left = t->left;
n->right = t->right;
n->level = t->level;
}
skew(T);
split(T);
return old;
}
tmp<volScalarField> realizableKE::rCmu
(
const volTensorField& gradU,
const volScalarField& S2,
const volScalarField& magS
)
{
tmp<volSymmTensorField> tS = dev(symm(gradU));
const volSymmTensorField& S = tS();
volScalarField W =
(2*sqrt(2.0))*((S&S)&&S)
/(
magS*S2
+ dimensionedScalar("small", dimensionSet(0, 0, -3, 0, 0), SMALL)
);
tS.clear();
volScalarField phis =
(1.0/3.0)*acos(min(max(sqrt(6.0)*W, -scalar(1)), scalar(1)));
volScalarField As = sqrt(6.0)*cos(phis);
volScalarField Us = sqrt(S2/2.0 + magSqr(skew(gradU)));
return 1.0/(A0_ + As*Us*k_/(epsilon_ + epsilonSmall_));
}
Matrix6d adjoint_se3(Matrix4d T){
Matrix6d AdjT = Matrix6d::Zero();
Matrix3d R = T.block(0,0,3,3);
AdjT.block(0,0,3,3) = R;
AdjT.block(0,3,3,3) = skew( T.block(0,3,3,1) ) * R ;
AdjT.block(3,3,3,3) = R;
return AdjT;
}
static void bstree_copy_sort( bstree bst, unsigned int node,
bstree_node *array, unsigned int *index){
static int bstree_shrink( bstree bst);
static int bstree_node_find( const bstree bst, const unsigned int *node,
KEYTYPE key, VALUETYPE *value){
if( key == bst->array[*node].key ){
*value = bst->array[*node].value;
return BSTREE_SUCCESS;
}
if( COMP(key, bst->array[*node].key) < 0 ){
if( bst->array[*node].left != NODE_NULL ){
return bstree_node_find( bst, &(bst->array[node].left), key, value);
}
}else{
if( bst->array[node].right != NODE_NULL ){
return bstree_node_find( bst, &(bst->array[node].right), key, value);
}
}
return BSTREE_FAILURE;
}
static int bstree_node_insert( bstree bst, unsigned int *node,
KEYTYPE key, VALUETYPE value){
int status;
if( *node == NODE_NULL){
if( bst->max_pos == bst->array_size ){
if( bstree_grow( bst) == BSTREE_MEM_ERROR ){
return BSTREE_MEM_ERROR;
}
}
*node = bstree->max_pos;
bst->array[*node].left = NODE_NULL;
bst->array[*node].right = NODE_NULL;
bst->array[*node].key = key;
bst->array[*node].value = value;
bst->array[*node].level = 1;
(bst->max_pos)++;
(bst->size)++;
return BSTREE_SUCCESS;
}
if( key == bst->array[*node].key ){
bst->array[*node].value += value;
return BSTREE_SUCCESS;
}
if( COMP( key, bst->array[*node].key) < 0 ){
status = bstree_node_insert( bst, &(bst->array[node].left), key, value);
}else{
status = bstree_node_insert( bst, &(bst->array[node].right), key, value);
}
skew( bst, node);
split( bst, node);
return status;
}
dimensionedTensor skew(const dimensionedTensor& dt)
{
return dimensionedTensor
(
"skew("+dt.name()+')',
dt.dimensions(),
skew(dt.value())
);
}
struct aa_tree_node *aa_tree_node_rebalance( struct aa_tree_node *t )
{
if( aa_tree_node_check_levels( t ) ) {
--t->level_;
if( t->links_[AA_LINK_RIGHT]->level_ > t->level_ )
t->links_[AA_LINK_RIGHT]->level_ = t->level_;
t = skew ( t );
t->links_[AA_LINK_RIGHT] = skew ( t->links_[AA_LINK_RIGHT] );
t->links_[AA_LINK_RIGHT]
->links_[AA_LINK_RIGHT] = skew ( t->links_[AA_LINK_RIGHT]
->links_[AA_LINK_RIGHT] );
t = split( t );
t->links_[AA_LINK_RIGHT] = split( t->links_[AA_LINK_RIGHT] );
}
return t;
}
void AATree<Comparable>::
remove( const Comparable & x, AANode<Comparable> * & t )
{
static AANode<Comparable> *lastNode, *deletedNode = nullNode;
if( t != nullNode )
{
// Step 1: Search down the tree and set lastNode and deletedNode
lastNode = t;
if( x < t->element )
remove( x, t->left );
else
{
deletedNode = t;
remove( x, t->right );
}
// Step 2: If at the bottom of the tree and
// x is present, we remove it
if( t == lastNode )
{
if( deletedNode == nullNode || x != deletedNode->element )
return; // Item not found; do nothing
deletedNode->element = t->element;
deletedNode = nullNode;
t = t->right;
delete lastNode;
}
// Step 3: Otherwise, we are not at the bottom; rebalance
else
if( t->left->level < t->level - 1 ||
t->right->level < t->level - 1 )
{
if( t->right->level > --t->level )
t->right->level = t->level;
skew( t );
skew( t->right );
skew( t->right->right );
split( t );
split( t->right );
}
}
}
static js_Property *jdelete(js_State *J, js_Object *obj, js_Property *node, const char *name)
{
js_Property *temp, *succ;
if (node != &sentinel) {
int c = strcmp(name, node->name);
if (c < 0) {
node->left = jdelete(J, obj, node->left, name);
} else if (c > 0) {
node->right = jdelete(J, obj, node->right, name);
} else {
if (node->left == &sentinel) {
temp = node;
node = node->right;
freeproperty(J, obj, temp);
} else if (node->right == &sentinel) {
temp = node;
node = node->left;
freeproperty(J, obj, temp);
} else {
succ = node->right;
while (succ->left != &sentinel)
succ = succ->left;
node->name = succ->name;
node->atts = succ->atts;
node->value = succ->value;
node->right = jdelete(J, obj, node->right, succ->name);
}
}
if (node->left->level < node->level - 1 ||
node->right->level < node->level - 1)
{
if (node->right->level > --node->level)
node->right->level = node->level;
node = skew(node);
node->right = skew(node->right);
node->right->right = skew(node->right->right);
node = split(node);
node->right = split(node->right);
}
}
return node;
}
Eigen::Matrix <Scalar_t,6,3> se3Action (const SE3 & m) const
{
Eigen::Matrix <double,6,3> X_subspace;
X_subspace.block <3,2> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ().leftCols <2> ();
X_subspace.block <3,1> (Motion::LINEAR, 2) = m.rotation ().rightCols <1> ();
X_subspace.block <3,2> (Motion::ANGULAR, 0) = m.rotation ().leftCols <2> ();
X_subspace.block <3,1> (Motion::ANGULAR, 2).setZero ();
return X_subspace;
}
cv::Mat findRotation(cv::Mat v1, cv::Mat v2){
cv::Mat ab = v1.cross(v2);
//std::cout << "cross " << ab << std::endl;
float s = sqrt(ab.at<float>(0,0)*ab.at<float>(0,0) + ab.at<float>(1,0)*ab.at<float>(1,0) + ab.at<float>(2,0)*ab.at<float>(2,0));
if (s == 0)
return cv::Mat::eye(3,3,CV_32F);
float c = v1.at<float>(0,0)*v2.at<float>(0,0) + v1.at<float>(1,0)*v2.at<float>(1,0) + v1.at<float>(2,0)*v2.at<float>(2,0);
cv::Mat sk = skew(ab);
return cv::Mat::eye(3,3,CV_32F) + sk + sk*sk*(1-c)/(s*s);
}
Matrix3d fast_skewexp(Vector3d v){
Matrix3d M, s, I = Matrix3d::Identity();
double theta = v.norm();
if(theta==0.f)
M = I;
else{
s = skew(v)/theta;
M << I + s * sin(theta) + s * s * (1.f-cos(theta));
}
return M;
}
int main() {
double first;
int i;
printf("Clock skew test\n");
for ( i = 0; i < 30; i++){
first = skew();
printf("skew %f \n",first);
sleep(few_seconds);
}
by_now();
}
matrix_t A_stance(cref_T_rotation_t contacts, cref_vector_t positions)
{
int nbContacts = (int)contacts.rows() / 3;
assert(contacts.rows() % 3 == 0 && contacts.rows() == positions.rows());
matrix_t mat = matrix_t::Zero(c_dim, nbContacts*c_dim);
for(int i = 0; i< nbContacts; ++i)
{
const cref_rotation_t& mRi = contacts.block<3,3>(3*i,0);
mat.block<3,3>(0,i*c_dim) = -mRi;
mat.block<3,3>(3,i*c_dim) = -skew(positions.segment<3>(3*i)) *mRi;
mat.block<3,3>(3,i*c_dim+3) = -mRi;
}
return mat;
}
Matrix3d v_logmap(VectorXd x){
Vector3d w;
double theta, theta2, theta3;
Matrix3d W, I, V;
w << x(0), x(1), x(2);
theta = w.norm(); theta2 = theta*theta; theta3 = theta2*theta;
W = skew(w);
I << 1, 0, 0, 0, 1, 0, 0, 0, 1;
if(theta>0.00001)
V << I + ((1-cos(theta))/theta2)*W + ((theta-sin(theta))/theta3)*W*W;
else
V << I;
return V;
}
/**
* Internal method to insert into a subtree.
* x is the item to insert.
* t is the node that roots the tree.
* Set the new root of the subtree.
*/
void insert( const Comparable & x, AANode * & t )
{
if( t == nullNode )
t = new AANode( x, nullNode, nullNode );
else if( x < t->element )
insert( x, t->left );
else if( t->element < x )
insert( x, t->right );
else
return; // Duplicate; do nothing
skew( t );
split( t );
}
/* remove is bit more tricky */
static Node *rebalance_on_remove(Node *current)
{
/*
* Removal can create a gap in levels,
* fix it by lowering current->level.
*/
if (current->left->level < current->level - 1
|| current->right->level < current->level - 1)
{
current->level--;
/* if ->right is red, change it's level too */
if (current->right->level > current->level)
current->right->level = current->level;
/* reshape, ask Arne about those */
current = skew(current);
current->right = skew(current->right);
current->right->right = skew(current->right->right);
current = split(current);
current->right = split(current->right);
}
return current;
}
static js_Property *insert(js_State *J, js_Object *obj, js_Property *node, const char *name, js_Property **result)
{
if (node != &sentinel) {
int c = strcmp(name, node->name);
if (c < 0)
node->left = insert(J, obj, node->left, name, result);
else if (c > 0)
node->right = insert(J, obj, node->right, name, result);
else
return *result = node;
node = skew(node);
node = split(node);
return node;
}
return *result = newproperty(J, obj, name);
}
int insert(int root, int val) {
if (A[root].val == -1)
A[root].val=val;
else if (A[root].val < val) // <= if duplicate ok
insert(A[root].right, val);
else if (A[root].val > val) // <= if duplicate ok
insert(A[root].left, val);
// insert a node with key value val in the
// (sub)tree rooted at root
int k=skew(root);
int j=split(k);
printf("INSERTED");
// return the index of the (possibly modified)
// root of the subtree
return j;
}
Matrix4d expmap_se3(Vector6d x){
Matrix3d R, V, s, I = Matrix3d::Identity();
Vector3d t, w;
Matrix4d T = Matrix4d::Identity();
w = x.tail(3);
t = x.head(3);
double theta = w.norm();
if( theta < 0.000001 )
R = I;
else{
s = skew(w)/theta;
R = I + s * sin(theta) + s * s * (1.0f-cos(theta));
V = I + s * (1.0f - cos(theta)) / theta + s * s * (theta - sin(theta)) / theta;
t = V * t;
}
T.block(0,0,3,4) << R, t;
return T;
}
/// solve for rotational Acceleration with No Constraints
Vect3 BodyRigid::solveRotAccel(bool storeAccels)
{
/// page 292 Nikravesh equation 11.18
Mat3x3 wskew = skew(m_wl);
/// TODO, see if line below is faster, or more accurate
//m_wldot = m_inertia.colPivHouseholderQr().solve(m_appliedTorque-wskew*m_inertia*m_wl);
//m_wldot = m_inertia.ldlt().solve(m_appliedTorque-wskew*m_inertia*m_wl);
Mat3x3 Iinv = getInverse(m_inertia);
Vect3 wldot = Iinv * (m_appliedTorque-wskew*m_inertia*m_wl);
if (storeAccels)
{
m_wldot = wldot;
}
return wldot;
}
void Foam::calc(const argList& args, const Time& runTime, const fvMesh& mesh)
{
IOobject Uheader
(
"U",
runTime.timeName(),
mesh,
IOobject::MUST_READ
);
if (Uheader.headerOk())
{
Info<< " Reading U" << endl;
volVectorField U(Uheader, mesh);
volTensorField gradU = fvc::grad(U);
volScalarField magD = mag(symm(gradU));
volScalarField magOmega = mag(skew(gradU));
dimensionedScalar smallMagD("smallMagD", magD.dimensions(), SMALL);
Info<< " Calculating flowType" << endl;
volScalarField flowType
(
IOobject
(
"flowType",
runTime.timeName(),
mesh,
IOobject::NO_READ
),
(magD - magOmega)/(magD + magOmega + smallMagD)
);
flowType.write();
}
else
{
Info<< " No U" << endl;
}
Info<< "\nEnd\n" << endl;
}
void PowerSum::dump(std::ostream& str) const throw()
{
str << "n:" << n;
for (int i=1; i<=order; i++)
str << " s" << i << ":" << s[i];
str << std::endl;
str << "m1:" << moment(1)
<< " m2:" << moment(2)
<< " m3:" << moment(3)
<< " m4:" << moment(4)
<< std::endl;
str << "average:" << average()
<< " stddev:" << sqrt(variance())
<< " skew:" << skew()
<< " kurtosis:" << kurtosis()
<< std::endl;
}
tmp<GeometricField<Type, fvPatchField, volMesh> >
curl
(
const GeometricField<Type, fvPatchField, volMesh>& vf
)
{
word nameCurlVf = "curl(" + vf.name() + ')';
// Gausses theorem curl
// tmp<GeometricField<Type, fvPatchField, volMesh> > tcurlVf =
// fvc::surfaceIntegrate(vf.mesh().Sf() ^ fvc::interpolate(vf));
// Calculate curl as the Hodge dual of the skew-symmetric part of grad
tmp<GeometricField<Type, fvPatchField, volMesh> > tcurlVf =
2.0*(*skew(fvc::grad(vf, nameCurlVf)));
tcurlVf().rename(nameCurlVf);
return tcurlVf;
}
