/*! 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; }