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