/**------------------------------------------------------------------------------- Sensors @brief @param simulation @return ---------------------------------------------------------------------------------*/ Sensors::Sensors(Simulation* simulation) { if (Config::Instance().getShowCOG()) { // create center-of-gravity (COG) sensor SensorVectors* s = new SensorCOG(simulation); boost::shared_ptr<SensorVectors> cog( new SensorDecoratorCross(s, Ogre::ColourValue(0, 0, 1), Config::Instance().getDrawingScaleCOG())); mSensors.push_back(cog); } else { boost::shared_ptr<SensorVectors> cog(new SensorCOG(simulation)); mSensors.push_back(cog); } // create center-of-pressure (COP) sensor if (Config::Instance().getShowCOP()) { SensorVectors* s = new SensorCOP(simulation); boost::shared_ptr<SensorVectors> cop( new SensorDecoratorCross(s, Ogre::ColourValue(0, 1, 0), Config::Instance().getDrawingScaleCOP())); mSensors.push_back(cop); } else { boost::shared_ptr<SensorVectors> cop(new SensorCOP(simulation)); mSensors.push_back(cop); } // create (Zero-moment-point) ZMP sensor if (Config::Instance().getShowZMP()) { SensorVectors* s = new SensorZMP(simulation); boost::shared_ptr<SensorVectors> zmp( new SensorDecoratorCross(s, Ogre::ColourValue(1, 0, 1), Config::Instance().getDrawingScaleZMP())); mSensors.push_back(zmp); } else { boost::shared_ptr<SensorVectors> zmp(new SensorZMP(simulation)); mSensors.push_back(zmp); } // create (Foot-rotation-index) FRI sensor if (Config::Instance().getShowFRI()) { SensorVectors* s = new SensorFRI(simulation); boost::shared_ptr<SensorVectors> fri( new SensorDecoratorCross(s, Ogre::ColourValue(0.42, 0.5624, 0.8492), Config::Instance().getDrawingScaleFRI())); mSensors.push_back(fri); } else { boost::shared_ptr<SensorVectors> fri(new SensorFRI(simulation)); mSensors.push_back(fri); } }
vec3 Grasp::virtualCentroid() { vec3 cog(0,0,0); position pos; /* //COG AS CENTROID for (int i=0; i<(int)contactVec.size(); i++) { pos = ((VirtualContact*)contactVec[i])->getWorldLocation(); cog = cog + vec3( pos.toSbVec3f() ); } cog = ( 1.0 / (int)contactVec.size() ) * cog; //fprintf(stderr,"CoG: %f %f %f\n",cog.x(), cog.y(), cog.z()); */ //COG as center of bounding box position topCorner(-1.0e5, -1.0e5, -1.0e5), bottomCorner(1.0e5, 1.0e5, 1.0e5); for (int i=0; i<(int)contactVec.size(); i++) { pos = ((VirtualContact*)contactVec[i])->getWorldLocation(); if ( pos.x() > topCorner.x() ) topCorner.x() = pos.x(); if ( pos.y() > topCorner.y() ) topCorner.y() = pos.y(); if ( pos.z() > topCorner.z() ) topCorner.z() = pos.z(); if ( pos.x() < bottomCorner.x() ) bottomCorner.x() = pos.x(); if ( pos.y() < bottomCorner.y() ) bottomCorner.y() = pos.y(); if ( pos.z() < bottomCorner.z() ) bottomCorner.z() = pos.z(); } cog = 0.5 * (topCorner - bottomCorner); cog = vec3(bottomCorner.toSbVec3f()) + cog; return cog; }
void paint(QPainter &painter){ painter.setRenderHint(QPainter::Antialiasing,true); const T w=painter.device()->width(); const T h=painter.device()->height(); QPen base(QBrush(Qt::NoBrush),2); QPen brown(base); brown.setWidthF(0.01); brown.setStyle(Qt::SolidLine); brown.setColor(QColor("brown")); QPen green(base); green.setWidthF(0.04); green.setColor(QColor("green")); QPen purple(base); purple.setWidthF(0.01); purple.setColor(QColor("purple").darker()); painter.translate(w/2,h/2); qreal s=qMin(w,h); painter.scale(s,s); T step=0.02; T gx=cx/step; gx-=floor(gx); gx*=-step; T gy=cy/step; gy-=floor(gy); gy*=-step; painter.setPen(brown); for(T y=gy-h;y<h;y+=step){ painter.drawLine(QPointF(0,y-1),QPointF(w,y+1)); } for(T x=gx-w;x<w;x+=step){ painter.drawLine(QPointF(x-1,0),QPointF(x+1,h)); } if(singleLimbMode){ limbs[0]->paint(painter); } else{ for(quint32 i=0;i<limbCount;++i){ limbs[i]->paint(painter); } const T r=0.01; QPointF cob(cobx,coby); painter.setPen(green); painter.drawEllipse(cob,r,r); QPointF cog(cogx,cogy); painter.setPen(purple); painter.drawEllipse(cog,r,r); } }
void dNewtonBody::CalculateBuoyancyForces(const void* plane, void* force, void* torque, float bodyDensity) { dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetMass(m_body, &mass, &Ixx, &Iyy, &Izz); if (mass > 0.0f) { dMatrix matrix; dVector cog(0.0f); dVector accelPerUnitMass(0.0f); dVector torquePerUnitMass(0.0f); const dVector gravity(0.0f, -9.8f, 0.0f, 0.0f); NewtonBodyGetMatrix(m_body, &matrix[0][0]); NewtonBodyGetCentreOfMass(m_body, &cog[0]); cog = matrix.TransformVector(cog); NewtonCollision* const collision = NewtonBodyGetCollision(m_body); dFloat shapeVolume = NewtonConvexCollisionCalculateVolume(collision); dFloat fluidDensity = 1.0f / (bodyDensity * shapeVolume); dFloat viscosity = 0.995f; NewtonConvexCollisionCalculateBuoyancyAcceleration(collision, &matrix[0][0], &cog[0], &gravity[0], (float*)plane, fluidDensity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]); dVector finalForce(accelPerUnitMass.Scale(mass)); dVector finalTorque(torquePerUnitMass.Scale(mass)); dVector omega(0.0f); NewtonBodyGetOmega(m_body, &omega[0]); omega = omega.Scale(viscosity); NewtonBodySetOmega(m_body, &omega[0]); ((float*)force)[0] = finalForce.m_x ; ((float*)force)[1] = finalForce.m_y ; ((float*)force)[2] = finalForce.m_z ; ((float*)torque)[0] = finalTorque.m_x; ((float*)torque)[1] = finalTorque.m_y; ((float*)torque)[2] = finalTorque.m_z; } }
void OnInside(NewtonBody* const visitor) { dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetMass(visitor, &mass, &Ixx, &Iyy, &Izz); if (mass > 0.0f) { dMatrix matrix; dVector cog(0.0f); dVector accelPerUnitMass(0.0f); dVector torquePerUnitMass(0.0f); const dVector gravity (0.0f, DEMO_GRAVITY, 0.0f, 0.0f); NewtonBodyGetMatrix (visitor, &matrix[0][0]); NewtonBodyGetCentreOfMass(visitor, &cog[0]); cog = matrix.TransformVector (cog); NewtonCollision* const collision = NewtonBodyGetCollision(visitor); dFloat shapeVolume = NewtonConvexCollisionCalculateVolume (collision); dFloat fluidDentity = 1.0f / (m_waterToSolidVolumeRatio * shapeVolume); dFloat viscosity = 0.995f; NewtonConvexCollisionCalculateBuoyancyAcceleration (collision, &matrix[0][0], &cog[0], &gravity[0], &m_plane[0], fluidDentity, viscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]); dVector force (accelPerUnitMass.Scale (mass)); dVector torque (torquePerUnitMass.Scale (mass)); dVector omega(0.0f); NewtonBodyGetOmega(visitor, &omega[0]); omega = omega.Scale (viscosity); NewtonBodySetOmega(visitor, &omega[0]); NewtonBodyAddForce (visitor, &force[0]); NewtonBodyAddTorque (visitor, &torque[0]); } }
template <typename PointInT, typename PointOutT> void pcl::ESFEstimation<PointInT, PointOutT>::scale_points_unit_sphere ( const pcl::PointCloud<PointInT> &pc, float scalefactor, Eigen::Vector4f& centroid) { pcl::compute3DCentroid (pc, centroid); pcl::demeanPointCloud (pc, centroid, local_cloud_); float max_distance = 0, d; pcl::PointXYZ cog (0, 0, 0); for (size_t i = 0; i < local_cloud_.points.size (); ++i) { d = pcl::euclideanDistance(cog,local_cloud_.points[i]); if (d > max_distance) max_distance = d; } float scale_factor = 1.0f / max_distance * scalefactor; Eigen::Affine3f matrix = Eigen::Affine3f::Identity(); matrix.scale (scale_factor); pcl::transformPointCloud (local_cloud_, local_cloud_, matrix); }
int main() { // Mammals mammal; // cannot instantiate abstract class /* Human human("John", 70, 2, 33.6, false); Dog dog("Ilie", 12, 4, 20.4); std::cout << '\n' << human << "\nwhile " << dog << "\n\n";*/ // Cog was inherited from both Cat and Dog Cog cog("Cog", 1, 7, 324.99); // will call it's own destructor // the dctor of the Cat and Dog (notice the reverse order) // the dctor of Mammals // (reverse order of how it was constructedd) /* human.print_organs(); // calls the static Mammals::print_organs() human.breath(); // breathe using Mammal base class functionality human.talk(); human.walk(); human.attack(); Cat cat; cat.breath(); cat.talk(); cat.walk(); Dog dog; dog.breath(); dog.talk(); dog.walk(); */ }
bool vpFaceTrackerOkao::detect() { m_message.clear(); m_polygon.clear(); m_nb_objects = 0; m_faces.clear(); m_scores.clear(); bool target_found = false; AL::ALValue result = m_mem_proxy.getData("FaceDetected"); //-- Detect faces if (result.getSize() >=2) { //std::cout << "face" << std::endl; AL::ALValue info_face_array = result[1]; target_found = true; double min_dist = m_image_width*m_image_height; unsigned int index_closest_cog = 0; vpImagePoint closest_cog; for (unsigned int i = 0; i < info_face_array.getSize()-1; i++ ) { //Extract face info // Face Detected [1]/ First face [0]/ Shape Info [0]/ Alpha [1] float alpha = result[1][i][0][1]; float beta = result[1][i][0][2]; float sx = result[1][i][0][3]; float sy = result[1][i][0][4]; std::string name = result[1][i][1][2]; float score = result[1][i][1][1]; std::ostringstream message; if (score > 0.6) message << name; else message << "Unknown"; m_message.push_back( message.str() ); m_scores.push_back(score); // sizeX / sizeY are the face size in relation to the image float h = m_image_height * sx; float w = m_image_width * sy; // Center of face into the image float x = m_image_width / 2 - m_image_width * alpha; float y = m_image_height / 2 + m_image_height * beta; vpImagePoint cog(x,y); double dist = vpImagePoint::distance(m_previuos_cog,cog); if (dist< min_dist) { closest_cog = cog; index_closest_cog = i; min_dist = dist; } std::vector<vpImagePoint> polygon; double x_corner = x - h/2; double y_corner = y - w/2; polygon.push_back(vpImagePoint(y_corner , x_corner )); polygon.push_back(vpImagePoint(y_corner+w, x_corner )); polygon.push_back(vpImagePoint(y_corner+w, x_corner+h)); polygon.push_back(vpImagePoint(y_corner , x_corner+h)); m_polygon.push_back(polygon); m_nb_objects ++; } if (index_closest_cog !=0) std::swap(m_polygon[0], m_polygon[index_closest_cog]); m_previuos_cog = closest_cog; } return target_found; }
void Customer::fireCogChanged() { cogChanged(cog()); marginChanged(margin()); }
colvar::orientation::orientation(std::string const &conf) : cvc(conf) { function_type = "orientation"; parse_group(conf, "atoms", atoms); atom_groups.push_back(&atoms); x.type(colvarvalue::type_quaternion); ref_pos.reserve(atoms.size()); if (get_keyval(conf, "refPositions", ref_pos, ref_pos)) { cvm::log("Using reference positions from input file.\n"); if (ref_pos.size() != atoms.size()) { cvm::fatal_error("Error: reference positions do not " "match the number of requested atoms.\n"); } } { std::string file_name; if (get_keyval(conf, "refPositionsFile", file_name)) { std::string file_col; double file_col_value=0.0; if (get_keyval(conf, "refPositionsCol", file_col, std::string(""))) { // use PDB flags if column is provided bool found = get_keyval(conf, "refPositionsColValue", file_col_value, 0.0); if (found && file_col_value==0.0) cvm::fatal_error("Error: refPositionsColValue, " "if provided, must be non-zero.\n"); } else { // if not, use atom indices atoms.create_sorted_ids(); } ref_pos.resize(atoms.size()); cvm::load_coords(file_name.c_str(), ref_pos, atoms.sorted_ids, file_col, file_col_value); } } if (!ref_pos.size()) { cvm::fatal_error("Error: must define a set of " "reference coordinates.\n"); } cvm::log("Centering the reference coordinates: it is " "assumed that each atom is the closest " "periodic image to the center of geometry.\n"); cvm::rvector cog(0.0, 0.0, 0.0); size_t i; for (i = 0; i < ref_pos.size(); i++) { cog += ref_pos[i]; } cog /= cvm::real(ref_pos.size()); for (i = 0; i < ref_pos.size(); i++) { ref_pos[i] -= cog; } get_keyval(conf, "closestToQuaternion", ref_quat, cvm::quaternion(1.0, 0.0, 0.0, 0.0)); // initialize rot member data if (!atoms.noforce) { rot.request_group2_gradients(atoms.size()); } }