int main(){
struct point a = { 0, 0, -14 };
struct point b = { 0, .01, -14 };
double angles1[3];
double angles2[3];
getAngles(angles1, a);
printf("%f, %f, %f\n", angles1[0], angles1[1], angles1[2]);
getAngles(angles2, b);
printf("%f, %f, %f\n", angles2[0], angles2[1], angles2[2]);
printf("%f, %f, %f\n", angles1[0] - angles2[0], angles1[1] - angles2[1], angles1[2] - angles2[2]);
//compareOldAndNew(.1);
//printTableAccuracy(1);
//RunSpeedTest_TablevsKin(.1);
//RunSpeedTest_Acos(1000000);
//double list[3] = { 1.0, 2.0, 3.0 };
//double list2[3] = { 0,0,0 };
//FILE stream;
//fwrite(list,sizeof(list[0]), sizeof(list[0]), &stream);
//fread(list2, sizeof(list[0]), sizeof(list[0]), &stream);
//printf("%f, %f, %f", list2[0], list2[1], list2[3]);
while(1){}
return 0;
}
/**
* @return Angle of the measured dimension.
*/
double RS_DimAngular::getAngle()
{
double ang1 = 0.0;
double ang2 = 0.0;
bool reversed = false;
RS_Vector p1;
RS_Vector p2;
getAngles(ang1, ang2, reversed, p1, p2);
if (!reversed)
{
if (ang2<ang1)
{
ang2+=2*M_PI;
}
return ang2-ang1;
}
else
{
if (ang1<ang2)
{
ang1+=2*M_PI;
}
return ang1-ang2;
}
}
void
dxJointUniversal::getInfo1( dxJoint::Info1 *info )
{
info->nub = 4;
info->m = 4;
bool limiting1 = ( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) &&
limot1.lostop <= limot1.histop;
bool limiting2 = ( limot2.lostop >= -M_PI || limot2.histop <= M_PI ) &&
limot2.lostop <= limot2.histop;
// We need to call testRotationLimit() even if we're motored, since it
// records the result.
limot1.limit = 0;
limot2.limit = 0;
if ( limiting1 || limiting2 )
{
dReal angle1, angle2;
getAngles( &angle1, &angle2 );
if ( limiting1 )
limot1.testRotationalLimit( angle1 );
if ( limiting2 )
limot2.testRotationalLimit( angle2 );
}
if ( limot1.limit || limot1.fmax > 0 ) info->m++;
if ( limot2.limit || limot2.fmax > 0 ) info->m++;
}
void giPointer::render()
{
Rotation *r = getAngles(position,_pointAt);
glPushMatrix();
glTranslatef(position->x, position->y, position->z);
glPushMatrix();
glRotatef(pitch, 0,0,1);
glRotatef(yaw, 0,1,0);
GLUquadricObj *quadObj = gluNewQuadric();
gluQuadricDrawStyle(quadObj, GLU_FILL);
gluQuadricNormals(quadObj, GLU_SMOOTH);
gluQuadricOrientation(quadObj, GLU_OUTSIDE);
glColor3f(0,0,1);
gluCylinder(quadObj, 0.05f, 0.0f, 0.1f, 10, 10);
glPushMatrix();
glTranslatef(0,0,-0.1f);
glColor3f(0,1,1);
gluCylinder(quadObj, 0.02f, 0.02f, 0.1f, 10, 10);
glPopMatrix();
glPopMatrix();
glPopMatrix();
}
void RunSpeedTest_TablevsKin(double stepsize){
clock_t start = 0;
clock_t duration;
double x,y,z;
double angles[3];
struct point p = { 0, 0, 0 };
int count = 0;
/*
start = clock();
for (x = X_TABLE_MIN; x <= X_TABLE_MAX; x += stepsize){
for (y = Y_TABLE_MIN; y <= Y_TABLE_MAX; y += stepsize){
for (z = Z_TABLE_MIN; z <= Z_TABLE_MAX; z += stepsize){
//printf("%f", x);
p.x = x;
p.y = y;
p.z = z;
getAnglesNew(angles, p);
count++;
}
}
}
printf("\n");
duration = ((clock_t)clock() - start); //(double)CLOCKS_PER_SEC;
printf("Duration: %ld \n", duration);
printf("Count: %i \n", count);
*/
start = clock();
for (x = X_TABLE_MIN; x <= X_TABLE_MAX; x += stepsize){
for (y = Y_TABLE_MIN; y <= Y_TABLE_MAX; y += stepsize){
for (z = Z_TABLE_MIN; z <= Z_TABLE_MAX; z += stepsize){
//printf("%f", x);
p.x = x;
p.y = y;
p.z = z;
getAngles(angles, p);
}
}
}
printf("\n");
duration = ((clock_t)clock() - start); //(double)CLOCKS_PER_SEC;
printf("Duration: %ld \n", duration);
start = clock();
for (x = X_TABLE_MIN; x <= X_TABLE_MAX; x += stepsize){
for (y = Y_TABLE_MIN; y <= Y_TABLE_MAX; y += stepsize){
for (z = Z_TABLE_MIN; z <= Z_TABLE_MAX; z += stepsize){
//printf("%f", x);
p.x = x;
p.y = y;
p.z = z;
lookupAngles(INVERSE_TABLE, angles, p);
}
}
}
printf("\n");
duration = ((clock_t)clock() - start); //(double)CLOCKS_PER_SEC;
printf("Duration: %ld \n", duration);
}
void Camera::pitchDownDiscrete() {
Vector3 angles = getAngles();
angles[CAMERA_PITCH] -= SPEED_TURN;
if (angles[CAMERA_PITCH] < -90)
angles[CAMERA_PITCH] = -90;
setAngles(angles);
}
void Camera::pitchUpDiscrete() {
Vector3 angles = getAngles();
angles[CAMERA_PITCH] += SPEED_TURN;
if (angles[CAMERA_PITCH] > 90)
angles[CAMERA_PITCH] = 90;
setAngles(angles);
}
float PolarDiagram::getGybeAngle(const float & speed){
if(!contains(speed)){
if(debug)
qDebug() << Q_FUNC_INFO << QString("(%1): speed not found").arg(QString::number(speed));
int i = 0;
// List of all the speeds available in the diagram
// given in ASC order
QList<float> speeds = diagram.keys();
while(speeds[i] < speed && i < speeds.size() - 1)
++i;
float maxang;
// input speed is bigger than any available
if(i == speeds.size() - 1)
maxang = getGybeAngle(speeds.last());
// input speed is smaller than any available
else if (i == 0)
maxang = getGybeAngle(speeds.first());
// input speed is between other available speeds
else
maxang = (getGybeAngle(speeds[i-1]) + getGybeAngle(speeds[i])) / 2;
return maxang;
}
// input speed is contained in our diagram
Angles aux = getAngles(speed);
// List of available angles for this speed
// List is in ASC order
QList<float> angles = aux.keys();
float maxta = 0, maxang = 0, ta, ang;
bool on_gybe = true;
for(int i = angles.size() - 1; on_gybe && i >= 0; --i){
ang = angles[i];
ta = aux.value(ang);
if(ang < MIN_ANGLE_CONSIDERED_GYBE)
on_gybe = false;
else
if(ta > maxta){
maxta = ta;
maxang = ang;
}
}
if(debug)
qDebug() << Q_FUNC_INFO << QString("(%1): %2").arg(QString::number(speed), QString::number(maxang));
return maxang;
}
GetRotateAngleFunctor::GetRotateAngleFunctor( const GeometrySpring * geometrySpring ) :
m_GeometrySpring( geometrySpring ), m_RotateAngle( 0 )
{
GeometrySpringGetAngles getAngles( m_GeometrySpring );
m_AngleFrom = getAngles.getLinkFromAngle();
m_AngleTo = getAngles.getLinkToAngle();
GetAnglesRangeBy2PointsPredicate getRange( m_AngleFrom, m_AngleTo, geometrySpring->getIsClosedPath() );
int minAngle = getRange.getMinAngle();
int maxAngle = getRange.getMaxAngle();
m_RotateAngle = maxAngle - minAngle;
}
float PolarDiagram::getTA(const float & speed, const float & angle){
if(!contains(speed)){
int i = 0;
// List of ALL the speeds avilable in de diagram
// given in ASC order
QList<float> speeds = diagram.keys();
while(speeds.at(i) < speed && i < speeds.size() - 1)
++i;
float ta;
// input speed is bigger than any available
if(i == speeds.size() - 1)
ta = getTA(speeds.last(), angle);
// input speed is smaller than any available
else if(i == 0)
ta = getTA(speeds.first(), angle);
// input speed is between other available speeds
else
ta = (getTA(speeds.at(i - 1), angle) + getTA(speeds.at(i), angle)) / 2;
return ta;
}
// input speed is contained in our diagram
Angles aux = getAngles(speed);
// List of available angles for this speed
// List is in ASC order
QList<float> angles = aux.keys();
int i = 0;
while(angles.at(i) < angle && i < angles.size() - 1)
++i;
// input angle is bigger than any available
if(i == angles.size() - 1)
return aux.value(angles.last());
// input angle is smaller than any available
else if(i == 0)
return aux.value(angles.first());
// input angle is between other available angles
else
return (aux.value(angles.at(i - 1)) + aux.value(angles.at(i))) / 2;
}
void PolarDiagram::setAngles(const float &speed, const Angles &angles){
if(debug)
qDebug() << Q_FUNC_INFO;
if(contains(speed)){
Angles merge = angles;
Angles aux = getAngles(speed);
Angles::const_iterator i = aux.constBegin();
while(i != aux.constEnd()){
merge.insert(i.key(), i.value());
++i;
}
diagram.insert(speed, merge);
} else
diagram.insert( speed, angles);
}
/*
void compareOldAndNew(double stepsize){
double avgDiff = 0;
double diff = 0;
double anglesOld[3];
double anglesNew[3];
int count = 0;
struct point p = { 0, 0, 0 };
for (double x = X_TABLE_MIN; x <= X_TABLE_MAX; x += stepsize){
for (double y = Y_TABLE_MIN; y <= Y_TABLE_MAX; y += stepsize){
for (double z = Z_TABLE_MIN; z <= Z_TABLE_MAX; z += stepsize){
p.x = x;
p.y = y;
p.z = z;
struct point p = { x, y, z };
getAngles(anglesOld, p);
//printf("%f\n", anglesOld[1]);
getAnglesNew(anglesNew, p);
//printf("%f\n", anglesNew[1]);
if (!(isnan(anglesOld[0]) | isnan(anglesOld[1]) | isnan(anglesOld[2]) |
isnan(anglesNew[0]) | isnan(anglesNew[1]) | isnan(anglesNew[2]))){
//printf("%f %f %f \n",x, y, z);
double diffX = anglesOld[0] - anglesNew[0];
double diffY = anglesOld[1] - anglesNew[1];
double diffZ = anglesOld[2] - anglesNew[2];
//printf("%f %f %f \n",diffX, diffY, diffZ);
diff += sqrt(diffX*diffX + diffY*diffY + diffZ*diffZ);
count++;
}
//printf("\n");
}
}
}
avgDiff /= count;
printf("AverageDiff: %f", diff);
}
*/
void printTableAccuracy(double stepsize){
printf("Checking LookupAngles Accuracy... \n");
double minDiff = 100000000;
double maxDiff = -100000000;
double avgDiff = 0;
double diff = 0;
double anglesTable[3];
double anglesKin[3];
int count = 0;
struct point p = {0,0,0};
for (double x = X_TABLE_MIN; x <= X_TABLE_MAX; x += stepsize){
for (double y = Y_TABLE_MIN; y <= Y_TABLE_MAX; y += stepsize){
for (double z = Z_TABLE_MIN; z <= Z_TABLE_MAX; z += stepsize){
//printf("%f %f %f \n",x, y, z);
p.x = x;
p.y = y;
p.z = z;
struct point p = { x, y, z };
getAngles(anglesKin, p);
lookupAngles(INVERSE_TABLE, anglesTable, p);
if (!(isnan(anglesKin[0]) | isnan(anglesKin[1]) | isnan(anglesKin[2]) |
isnan(anglesTable[0]) | isnan(anglesTable[1]) | isnan(anglesTable[2]))){
//printf("%f %f %f \n", anglesKin[0], anglesKin[1], anglesKin[2]);
//printf("%f %f %f \n\n", anglesTable[0], anglesTable[1], anglesTable[2]);
double diffX = anglesKin[0] - anglesTable[0];
double diffY = anglesKin[1] - anglesTable[1];
double diffZ = anglesKin[2] - anglesTable[2];
diff = sqrt(diffX*diffX + diffY*diffY + diffZ*diffZ);
printf("%f %f %f \n", x, y, z);
printf("%f %f %f \n\n", diffX, diffY, diffZ);
//printf("%f \n", avgDiff);
//printf("%f %i \n", avgDiff, count);
avgDiff += diff;
count++;
maxDiff = diff > maxDiff ? diff : maxDiff;
minDiff = diff < minDiff ? diff : minDiff;
}
}
}
}
avgDiff /= count;
printf("AverageDiff: %f MinDiff: %f MaxDiff: %f", avgDiff, minDiff, maxDiff);
}
void
dxJointPU::getInfo1( dxJoint::Info1 *info )
{
info->m = 3;
info->nub = 3;
// powered needs an extra constraint row
// see if we're at a joint limit.
limotP.limit = 0;
if (( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) &&
limotP.lostop <= limotP.histop )
{
// measure joint position
dReal pos = dJointGetPUPosition( this );
limotP.testRotationalLimit( pos ); // N.B. The function is ill named
}
if ( limotP.limit || limotP.fmax > 0 ) info->m++;
bool limiting1 = ( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) &&
limot1.lostop <= limot1.histop;
bool limiting2 = ( limot2.lostop >= -M_PI || limot2.histop <= M_PI ) &&
limot2.lostop <= limot2.histop;
// We need to call testRotationLimit() even if we're motored, since it
// records the result.
limot1.limit = 0;
limot2.limit = 0;
if ( limiting1 || limiting2 )
{
dReal angle1, angle2;
getAngles( &angle1, &angle2 );
if ( limiting1 )
limot1.testRotationalLimit( angle1 );
if ( limiting2 )
limot2.testRotationalLimit( angle2 );
}
if ( limot1.limit || limot1.fmax > 0 ) info->m++;
if ( limot2.limit || limot2.fmax > 0 ) info->m++;
}
void GeometrySpringDynamic::initSpring()
{
GeometrySpringGetAngles getAngles( this );
// int linkFromAbsoluteAngle = getAngles.getLinkFromAngle();
// int linkToAbsoluteAngle = getAngles.getLinkToAngle();
// float angle = M_PI / 180.0 * angleInt;
const GeometryLinkDynamic * linkFrom = getDynamicLinkFrom();
const GeometryLinkDynamic * linkTo = getDynamicLinkTo();
cpBody * bodyFrom = linkFrom->getBody();
cpBody * bodyTo = linkTo->getBody();
GetRotateAngleFunctor getAngle( this );
int rotateAngleInt = getAngle.getAngle();
float rotateAngle = M_PI / 180.0 * rotateAngleInt;
m_ConstraintGear = cpSpaceAddConstraint( m_Space, cpGearJointNew( bodyFrom, bodyTo, rotateAngle, 1.0f ) );
}
UP_ERROR PolarDiagram::check(){
if(debug)
qDebug() << Q_FUNC_INFO;
if(name.isEmpty())
return ERROR_PD_NAME;
if(diagram.size() == 0)
return ERROR_PD_SIZE;
QList<float> speeds = diagram.keys();
for(int i = 0; i < speeds.size(); i++){
// for each speed we check the list of angles-ta
// very simple not in deep check of the diagram
Angles aux = getAngles(speeds[i]);
QList<float> angles = aux.keys();
bool a_gybe_angle = false;
bool a_beat_angle = false;
float ang, ta;
for(int j = 0; j < angles.size(); j++){
ang = angles[j];
ta = aux.value(ang);
if(ang < MAX_ANGLE_CONSIDERED_BEAT)
a_beat_angle = true;
else if(ang > MIN_ANGLE_CONSIDERED_GYBE)
a_gybe_angle = true;
// just check if TA is a reasonable value
if(ta < MIN_TA_ALLOWED || ta > MAX_TA_ALLOWED)
return ERROR_PD_TA;
}
// at least one beat angle and one gybe angle
// is required in each list of angles-ta
if(!a_beat_angle || !a_gybe_angle)
return ERROR_PD_ANG;
}
return ERROR_NONE;
}
/**
* @return Angle of the measured dimension.
*/
double RS_DimAngular::getAngle() {
double ang1 = 0.0;
double ang2 = 0.0;
bool reversed = false;
RS_Vector p1;
RS_Vector p2;
getAngles(ang1, ang2, reversed, p1, p2);
return reversed ? RS_Math::correctAngle(ang1 - ang2) : RS_Math::correctAngle(ang2 - ang1);
//
// if (!reversed) {
// if (ang2<ang1) {
// ang2+=2*M_PI;
// }
// return ang2-ang1;
// } else {
// if (ang1<ang2) {
// ang1+=2*M_PI;
// }
// return ang1-ang2;
// }
}
/**
* Updates the sub entities of this dimension. Called when the
* dimension or the position, alignment, .. changes.
*
* @param autoText Automatically reposition the text label
*/
void RS_DimAngular::update(bool /*autoText*/) {
RS_DEBUG->print("RS_DimAngular::update");
clear();
if (isUndone()) {
return;
}
// distance from entities (DIMEXO)
double dimexo = getExtensionLineOffset();
// extension line extension (DIMEXE)
double dimexe = getExtensionLineExtension();
// text height (DIMTXT)
double dimtxt = getTextHeight();
// text distance to line (DIMGAP)
double dimgap = getDimensionLineGap();
// find out center:
RS_Vector center = getCenter();
if (!center.valid) {
return;
}
double ang1 = 0.0;
double ang2 = 0.0;
bool reversed = false;
RS_Vector p1;
RS_Vector p2;
getAngles(ang1, ang2, reversed, p1, p2);
double rad = edata.definitionPoint4.distanceTo(center);
RS_Line* line;
RS_Vector dir;
double len;
double dist;
// 1st extension line:
dist = center.distanceTo(p1);
len = rad - dist + dimexe;
dir.setPolar(1.0, ang1);
line = new RS_Line(this,
RS_LineData(center + dir*dist + dir*dimexo,
center + dir*dist + dir*len));
line->setPen(RS_Pen(RS2::FlagInvalid));
line->setLayer(NULL);
addEntity(line);
// 2nd extension line:
dist = center.distanceTo(p2);
len = rad - dist + dimexe;
dir.setPolar(1.0, ang2);
line = new RS_Line(this,
RS_LineData(center + dir*dist + dir*dimexo,
center + dir*dist + dir*len));
line->setPen(RS_Pen(RS2::FlagInvalid));
line->setLayer(NULL);
addEntity(line);
// Create dimension line (arc):
RS_Arc* arc = new RS_Arc(this,
RS_ArcData(center,
rad, ang1, ang2, reversed));
arc->setPen(RS_Pen(RS2::FlagInvalid));
arc->setLayer(NULL);
addEntity(arc);
// length of dimension arc:
double distance = arc->getLength();
// do we have to put the arrows outside of the arc?
bool outsideArrows = (distance<getArrowSize()*2);
// arrow angles:
double arrowAngle1, arrowAngle2;
double arrowAng;
if (rad>1.0e-6) {
arrowAng = getArrowSize() / rad;
}
else {
arrowAng = 0.0;
}
RS_Vector v1, v2;
if (!arc->isReversed()) {
v1.setPolar(rad, arc->getAngle1()+arrowAng);
} else {
v1.setPolar(rad, arc->getAngle1()-arrowAng);
}
v1+=arc->getCenter();
arrowAngle1 = arc->getStartpoint().angleTo(v1);
if (!arc->isReversed()) {
v2.setPolar(rad, arc->getAngle2()-arrowAng);
} else {
v2.setPolar(rad, arc->getAngle2()+arrowAng);
}
v2+=arc->getCenter();
arrowAngle2 = arc->getEndpoint().angleTo(v2);
if (!outsideArrows) {
arrowAngle1 = arrowAngle1+M_PI;
arrowAngle2 = arrowAngle2+M_PI;
}
// Arrows:
RS_SolidData sd;
RS_Solid* arrow;
// arrow 1
arrow = new RS_Solid(this, sd);
arrow->shapeArrow(arc->getStartpoint(),
arrowAngle1,
getArrowSize());
arrow->setPen(RS_Pen(RS2::FlagInvalid));
arrow->setLayer(NULL);
addEntity(arrow);
// arrow 2:
arrow = new RS_Solid(this, sd);
arrow->shapeArrow(arc->getEndpoint(),
arrowAngle2,
getArrowSize());
arrow->setPen(RS_Pen(RS2::FlagInvalid));
arrow->setLayer(NULL);
addEntity(arrow);
// text label:
RS_TextData textData;
RS_Vector textPos = arc->getMiddlePoint();
RS_Vector distV;
double textAngle;
double dimAngle1 = textPos.angleTo(arc->getCenter())-M_PI/2.0;
// rotate text so it's readable from the bottom or right (ISO)
// quadrant 1 & 4
if (dimAngle1>M_PI/2.0*3.0+0.001 ||
dimAngle1<M_PI/2.0+0.001) {
distV.setPolar(dimgap, dimAngle1+M_PI/2.0);
textAngle = dimAngle1;
}
// quadrant 2 & 3
else {
distV.setPolar(dimgap, dimAngle1-M_PI/2.0);
textAngle = dimAngle1+M_PI;
}
// move text away from dimension line:
textPos+=distV;
textData = RS_TextData(textPos,
dimtxt, 30.0,
RS2::VAlignBottom,
RS2::HAlignCenter,
RS2::LeftToRight,
RS2::Exact,
1.0,
getLabel(),
"standard",
textAngle);
RS_Text* text = new RS_Text(this, textData);
// move text to the side:
text->setPen(RS_Pen(RS2::FlagInvalid));
text->setLayer(NULL);
addEntity(text);
calculateBorders();
}
void Camera::rotateRightDiscrete() {
Vector3 angles = getAngles();
angles[CAMERA_YAW] -= SPEED_TURN;
setAngles(angles);
}
QList<QSharedPointer<RShape> > RDimAngularData::getShapes(const RBox& queryBox, bool ignoreComplex, bool segment) const {
Q_UNUSED(queryBox)
Q_UNUSED(ignoreComplex)
Q_UNUSED(segment)
QSharedPointer<RBlockReferenceEntity> dimBlockReference = getDimensionBlockReference();
if (!dimBlockReference.isNull()) {
return dimBlockReference->getShapes(queryBox, ignoreComplex);
}
QList<QSharedPointer<RShape> > ret;
double dimexo = getDimexo();
double dimexe = getDimexe();
double dimtxt = getDimtxt();
double dimgap = getDimgap();
double dimasz = getDimasz();
// find out center:
RVector center = getCenter();
if (!center.isValid()) {
return ret;
}
double ang1 = 0.0;
double ang2 = 0.0;
bool reversed = false;
RVector p1;
RVector p2;
getAngles(ang1, ang2, reversed, p1, p2);
double rad = dimArcPosition.getDistanceTo(center);
RLine line;
RVector dir;
double len;
double dist;
// 1st extension line:
dist = center.getDistanceTo2D(p1);
len = rad - dist + dimexe;
dir.setPolar(1.0, ang1);
line = RLine(center + dir*dist + dir*dimexo, center + dir*dist + dir*len);
ret.append(QSharedPointer<RShape>(new RLine(line)));
// 2nd extension line:
dist = center.getDistanceTo2D(p2);
len = rad - dist + dimexe;
dir.setPolar(1.0, ang2);
line = RLine(center + dir*dist + dir*dimexo, center + dir*dist + dir*len);
ret.append(QSharedPointer<RShape>(new RLine(line)));
// Create dimension line (arc):
RArc arc(center, rad, ang1, ang2, reversed);
ret.append(QSharedPointer<RShape>(new RArc(arc)));
// length of dimension arc:
double distance = arc.getLength();
// do we have to put the arrows outside of the arc?
bool outsideArrows = (distance<dimasz*2);
// arrow angles:
double arrowAngle1, arrowAngle2;
double arrowAng;
if (rad>1.0e-6) {
arrowAng = getDimasz() / rad;
}
else {
arrowAng = 0.0;
}
if (outsideArrows) {
arrowAngle1 = arc.getDirection1();
arrowAngle2 = arc.getDirection2();
}
else {
RVector v1, v2;
if (!arc.isReversed()) {
v1.setPolar(rad, arc.getStartAngle()+arrowAng);
} else {
v1.setPolar(rad, arc.getStartAngle()-arrowAng);
}
v1+=arc.getCenter();
arrowAngle1 = arc.getStartPoint().getAngleTo(v1);
if (!arc.isReversed()) {
v2.setPolar(rad, arc.getEndAngle()-arrowAng);
} else {
v2.setPolar(rad, arc.getEndAngle()+arrowAng);
}
v2+=arc.getCenter();
arrowAngle2 = arc.getEndPoint().getAngleTo(v2);
arrowAngle1 = arrowAngle1+M_PI;
arrowAngle2 = arrowAngle2+M_PI;
}
// Arrows:
//RTriangle arrow = RTriangle::createArrow(arc.getStartPoint(), arrowAngle1, dimasz);
QList<QSharedPointer<RShape> > arrow = getArrow(arc.getStartPoint(), arrowAngle1);
ret.append(arrow);
//arrow = RTriangle::createArrow(arc.getEndPoint(), arrowAngle2, dimasz);
arrow = getArrow(arc.getEndPoint(), arrowAngle2);
ret.append(arrow);
//ret.append(QSharedPointer<RShape>(new RTriangle(arrow)));
//RVector oldMot = textPosition;
//textPosition = RVector(0,0);
//defaultAngle = 0.0;
//dimLineLength = RNANDOUBLE;
//getTextData();
//textPosition = oldMot;
RVector textPos = arc.getMiddlePoint();
double dimAngle1 = textPos.getAngleTo(arc.getCenter())-M_PI/2.0;
if (!autoTextPos) {
dimAngle1 = textPositionCenter.getAngleTo(arc.getCenter())-M_PI/2.0;
}
RVector distV;
double textAngle;
// rotate text so it's readable from the bottom or right (ISO)
// quadrant 1 & 4
if (dimAngle1>M_PI/2.0*3.0+0.001 ||
dimAngle1<M_PI/2.0+0.001) {
distV.setPolar(dimgap + dimtxt/2, dimAngle1+M_PI/2.0);
textAngle = dimAngle1;
}
// quadrant 2 & 3
else {
distV.setPolar(dimgap + dimtxt/2, dimAngle1-M_PI/2.0);
textAngle = dimAngle1+M_PI;
}
if (!autoTextPos) {
textPos = textPositionCenter;
} else {
// move text away from dimension line:
textPos+=distV;
textPositionCenter = textPos;
}
defaultAngle = textAngle;
//getTextData();
//textData.rotate(textAngle, RVector(0,0));
//textData.move(textPos);
return ret;
}
int main(int argc, char *argv[]) {
/////VARIABLE DECLARATIONS/////
//SENSORS
mraa_i2c_context accel, gyro, mag;
float a_res, g_res, m_res;
data_t accel_data, gyro_data, mag_data;
int16_t temperature;
float pitch_angle, yaw_angle;
char *x_accel_message;
char *y_accel_message;
char *z_accel_message;
char *posture_message;
int curr_posture;
int prev_posture = 0;
//SOCKETS AND MESSAGES
int sockfd; //Socket descriptor
int portno, n;
char component[256];
char endpoint[] = "127.0.0.1";
struct sockaddr_in serv_addr;
struct hostent *server; //struct containing a bunch of info
char message[256];
int count;
/////SENSOR INITIALIZATION AND SETUP
accel = accel_init();
set_accel_scale(accel, A_SCALE_2G);
set_accel_ODR(accel, A_ODR_100);
a_res = calc_accel_res(A_SCALE_2G);
gyro = gyro_init();
set_gyro_scale(gyro, G_SCALE_245DPS);
set_gyro_ODR(accel, G_ODR_190_BW_70);
g_res = calc_gyro_res(G_SCALE_245DPS);
mag = mag_init();
set_mag_scale(mag, M_SCALE_2GS);
set_mag_ODR(mag, M_ODR_125);
m_res = calc_mag_res(M_SCALE_2GS);
portno = 41234;
//create socket
sockfd = socket(AF_INET, SOCK_DGRAM, 0); //create a new socket
if (sockfd < 0)
error("ERROR opening socket");
server = gethostbyname("127.0.0.1"); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses...
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
//setup the server struct
memset((char *) &serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET; //initialize server's address
bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
serv_addr.sin_port = htons(portno);
//connect to server
if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server
error("ERROR connecting");
//read accel and gyro data
count = 0;
while(1) {
accel_data = read_accel(accel, a_res);
gyro_data = read_gyro(gyro, g_res);
//mag_data = read_mag(mag, m_res);
//temperature = read_temp(accel);
getAngles(accel_data, &pitch_angle, &yaw_angle);
printf("is moving: %d\n", isMoving(gyro_data) );
//printf("yaw angle: %f ", yaw_angle);
//printf("pitch angle: %f ", pitch_angle);
//printf("z vector: %f\n", accel_data.z);
if (count == 3) {
//send posture to cloud
if (isMoving(gyro_data)==0) //if patient is stationary, calculate new posture
curr_posture = getPosture(accel_data, pitch_angle, yaw_angle);
else
curr_posture = prev_posture; //else just use the old posture
if (curr_posture==UNDEFINED)
curr_posture = prev_posture;
prev_posture = curr_posture; //set new value for prev posture
posture_message = construct_message(POS, accel_data, curr_posture);
n = write(sockfd, posture_message, strlen(posture_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
//store accel data in string
/*
x_accel_message = construct_message(X_DIR, accel_data);
//printf("%s\n", full_message_x);
//send UDP message
n = write(sockfd,x_accel_message,strlen(x_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
y_accel_message = construct_message(Y_DIR, accel_data);
//printf("%s\n", full_message_y);
n = write(sockfd,y_accel_message,strlen(y_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
z_accel_message = construct_message(Z_DIR, accel_data);
//printf("%s\n", full_message_z);
n = write(sockfd,z_accel_message,strlen(z_accel_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
*/
count = 0;
}
char *posture_string;
switch(curr_posture)
{
case 0:
posture_string = "upright";
break;
case 1:
posture_string = "face up";
break;
case 2:
posture_string = "face down";
break;
case 3:
posture_string = "left";
break;
case 4:
posture_string = "right";
break;
default:
posture_string = "undefined";
}
printf("current orientation: %s \n", posture_string);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z);
//printf("\t%ld\n", temperature);
count++;
usleep(100000);
}
return 0;
}
uint32_t CB_CollisionArea::getAngles()
{
return getAngles(vectors);
}
CB_CollisionArea CB_CollisionArea::sweep(const CB_Vector2D& direction)
{
vector<CB_Vector2D*> optimizedVectors = optimize();
vector<CB_Vector2D*> optimizedVectors2 = optimize();
uint32_t angles = getAngles(optimizedVectors);
if(angles > 0)
{
optimizedVectors = reverse(optimizedVectors);
}
vector<CB_Vector2D*> vectors2;
float angle = ((CB_Vector2D)direction).getAngle();
float nAngle = angle + PI;
if(nAngle > PI)
{
nAngle-= 2 * PI;
}
for(uint32_t idx = 0; idx < optimizedVectors.size(); idx++)
{
uint32_t pIdx = (idx + optimizedVectors.size() - 1) % optimizedVectors.size();
CB_Vector2D * a = *(optimizedVectors.begin() + pIdx);
CB_Vector2D * b = *(optimizedVectors.begin() + idx);
CB_Vector2D * c = new CB_Vector2D(*a + direction);
CB_Vector2D * d = new CB_Vector2D(*b + direction);
CB_Vector2D * curVec = new CB_Vector2D(*b - *a);
double angleDiff = curVec->getAngle();
delete curVec;
if(angleDiff - angle > PI)
{
angleDiff-= 2 * PI;
}
if(angleDiff - angle < -PI)
{
angleDiff+= 2 * PI;
}
if(angleDiff >= angle)
{
if(vectors2.end() == vectors2.begin() || *(vectors2.end() - 1) != a)
{
vectors2.push_back(a);
}
vectors2.push_back(b);
}
curVec = new CB_Vector2D(*d - *c);
angleDiff = curVec->getAngle();
delete curVec;
if(angleDiff - nAngle > PI)
{
angleDiff-= 2 * PI;
}
if(angleDiff - nAngle < -PI)
{
angleDiff+= 2 * PI;
}
if(angleDiff >= nAngle)
{
if(vectors2.end() == vectors2.begin() || (((CB_Vector2D)**(vectors2.end() - 1)).x != c->x && ((CB_Vector2D)**(vectors2.end() - 1)).y != c->y))
{
vectors2.push_back(c);
}
else
{
delete c;
}
vectors2.push_back(d);
}
else
{
delete c;
delete d;
}
}
optimizedVectors.clear();
return CB_CollisionArea(vectors2);
}
void makeLikelihoodRotation(std::string inname, std::string outname, double SMOOTH, bool isAsimov=false){
gSystem->Load("libHiggsAnalysisCombinedLimit.so");
//TFile *fi = TFile::Open("lduscan_neg_ext/3D/lduscan_neg_ext_3D.root");
//TFile *fi = TFile::Open("lduscan_neg_ext_2/exp3D/lduscan_neg_ext_2_exp3D.root");
TFile *fi = TFile::Open(inname.c_str());
TTree *tree = (TTree*)fi->Get("limit");
//TTree *tree = new TTree("tree_vals","tree_vals");
// ------------------------------ THIS IS WHERE WE BUILD THE SPLINE ------------------------ //
// Create 2 Real-vars, one for each of the parameters of the spline
// The variables MUST be named the same as the corresponding branches in the tree
//
RooRealVar ldu("lambda_du","lambda_du",0.1,-2.5,2.5);
RooRealVar lVu("lambda_Vu","lambda_Vu",0.1,0,2.2);
RooRealVar kuu("kappa_uu","kappa_uu",0.1,0,2.2);
RooSplineND *spline = new RooSplineND("spline","spline",RooArgList(ldu,lVu,kuu),tree,"deltaNLL",SMOOTH,true,"deltaNLL >= 0 && deltaNLL < 500 && ( (TMath::Abs(quantileExpected)!=1 && TMath::Abs(quantileExpected)!=0) || (Entry$==0) )");
// ----------------------------------------------------------------------------------------- //
//TGraph *gr = spline->getGraph("x",0.1); // Return 1D graph. Will be a slice of the spline for fixed y generated at steps of 0.1
fOut = new TFile(outname.c_str(),"RECREATE");
// Plot the 2D spline
/*
TGraph2D *gcvcf = new TGraph2D(); gcvcf->SetName("cvcf");
TGraph2D *gcvcf_kuu = new TGraph2D(); gcvcf_kuu->SetName("cvcf_kuu");
TGraph2D *gcvcf_lVu = new TGraph2D(); gcvcf_lVu->SetName("cvcf_lVu");
*/
TGraph2D *type1_minscan = new TGraph2D();
type1_minscan->SetName("type1_minscan");
TGraph2D *type2_minscan = new TGraph2D();
type2_minscan->SetName("type2_minscan");
TGraph2D *gr_ldu = new TGraph2D(); gr_ldu->SetName("t1_ldu");
TGraph2D *gr_lVu = new TGraph2D(); gr_lVu->SetName("t1_lVu");
TGraph2D *gr_kuu = new TGraph2D(); gr_kuu->SetName("t1_kuu");
TGraph2D *gr2_ldu = new TGraph2D(); gr2_ldu->SetName("t2_ldu");
TGraph2D *gr2_lVu = new TGraph2D(); gr2_lVu->SetName("t2_lVu");
TGraph2D *gr2_kuu = new TGraph2D(); gr2_kuu->SetName("t2_kuu");
TGraph2D *gr_ku = new TGraph2D(); gr_ku->SetName("t1_ku");
TGraph2D *gr_kd = new TGraph2D(); gr_kd->SetName("t1_kd");
TGraph2D *gr_kV = new TGraph2D(); gr_kV->SetName("t1_kV");
TGraph2D *gr2_ku = new TGraph2D(); gr2_ku->SetName("t2_ku");
TGraph2D *gr2_kd = new TGraph2D(); gr2_kd->SetName("t2_kd");
TGraph2D *gr2_kV = new TGraph2D(); gr2_kV->SetName("t2_kV");
TGraph2D *gr_beta = new TGraph2D(); gr_beta->SetName("beta");
TGraph2D *gr_bma = new TGraph2D(); gr_bma->SetName("beta_minis_alpha");
// check the values of the three parameters during the scan ?!
double Vldu, VlVu, Vkuu; // holders for the values
int pt1,pt2 = 0;
double mint2 = 10000;
double mint1 = 10000;
double mint1_x = 10000;
double mint1_y = 10000;
double mint2_x = 10000;
double mint2_y = 10000;
double mint1_lVu = 10000;
double mint1_ldu = 10000;
double mint1_kuu = 10000;
double mint2_lVu = 10000;
double mint2_ldu = 10000;
double mint2_kuu = 10000;
int ccounter = 0;
double Vku, Vkd, VkV;
TGraph2D *g_FFS = new TGraph2D(); g_FFS->SetName("ffs_ldu_1");
int pt=0;
for (double x=0.;x<=3.0;x+=0.05){
for (double y=0.;y<=3.0;y+=0.05){
ldu.setVal(1);
lVu.setVal(y);
kuu.setVal(x);
double dnll2 = 2*spline->getVal();
g_FFS->SetPoint(pt,x,y,dnll2);
pt++;
}
}
if (!isAsimov){
double Vbma, Vbeta;
for (double cbma=-0.8;cbma<0.8;cbma+=0.01){
for (double b=0.1;b<1.4;b+=0.05){
double tanb = TMath::Tan(b);
getAngles(cbma,tanb,&Vbeta,&Vbma);
type1(cbma, tanb, &Vldu, &VlVu, &Vkuu);
type1_ex(cbma, tanb, &Vku, &Vkd, &VkV);
if (Vldu > ldu.getMax() || Vldu < ldu.getMin()) {
type1_minscan->SetPoint(ccounter,cbma,tanb,10);
}
if (VlVu > lVu.getMax() || VlVu < lVu.getMin()) {
type1_minscan->SetPoint(ccounter,cbma,tanb,10);
}
if (Vkuu > kuu.getMax() || Vkuu < kuu.getMin()) {
type1_minscan->SetPoint(ccounter,cbma,tanb,10);
} else {
ldu.setVal(Vldu);lVu.setVal(VlVu);kuu.setVal(Vkuu);
double dnll2 = 2*spline->getVal();
if (dnll2 < mint1) {
mint1_x = cbma;
mint1_y = tanb;
mint1 = dnll2;
mint1_lVu = VlVu;
mint1_kuu = Vkuu;
mint1_ldu = Vldu;
}
type1_minscan->SetPoint(ccounter,cbma,tanb,dnll2);
}
//std::cout << " Checking point cbma,tanb -> ldu, lVu, kuu == 2DeltaNLL " << cbma << ", " << tanb << " --> " << Vldu << ", " << VlVu << ", " << Vkuu << " == " << dnll2 << std::endl;
gr_ldu->SetPoint(ccounter,cbma,tanb,Vldu);
gr_lVu->SetPoint(ccounter,cbma,tanb,VlVu);
gr_kuu->SetPoint(ccounter,cbma,tanb,Vkuu);
gr_ku->SetPoint(ccounter,cbma,tanb,Vku);
gr_kd->SetPoint(ccounter,cbma,tanb,Vkd);
gr_kV->SetPoint(ccounter,cbma,tanb,VkV);
type2(cbma, tanb, &Vldu, &VlVu, &Vkuu);
type2_ex(cbma, tanb, &Vku, &Vkd, &VkV);
if (Vldu > ldu.getMax() || Vldu < ldu.getMin()) {
type2_minscan->SetPoint(ccounter,cbma,tanb,10);
}
if (VlVu > lVu.getMax() || VlVu < lVu.getMin()) {
type2_minscan->SetPoint(ccounter,cbma,tanb,10);
}
if (Vkuu > kuu.getMax() || Vkuu < kuu.getMin()) {
type2_minscan->SetPoint(ccounter,cbma,tanb,10);
} else {
ldu.setVal(Vldu);lVu.setVal(VlVu);kuu.setVal(Vkuu);
double dnll2 = 2*spline->getVal();
if (dnll2 < mint2) {
mint2_x = cbma;
mint2_y = tanb;
mint2 = dnll2;
mint2_lVu = VlVu;
mint2_kuu = Vkuu;
mint2_ldu = Vldu;
}
type2_minscan->SetPoint(ccounter,cbma,tanb,dnll2);
}
gr2_ldu->SetPoint(ccounter,cbma,tanb,Vldu);
gr2_lVu->SetPoint(ccounter,cbma,tanb,VlVu);
gr2_kuu->SetPoint(ccounter,cbma,tanb,Vkuu);
gr2_ku->SetPoint(ccounter,cbma,tanb,Vku);
gr2_kd->SetPoint(ccounter,cbma,tanb,Vkd);
gr2_kV->SetPoint(ccounter,cbma,tanb,VkV);
gr_beta->SetPoint(ccounter,cbma,tanb,Vbeta);
gr_bma->SetPoint(ccounter,cbma,tanb,Vbma);
ccounter++;
}
}
std::cout << "T1 Minimum found at " << mint1_x << "," << mint1_y << "( or in lVu, kuu, ldu) = " << mint1_lVu << ", " << mint1_kuu << ", " << mint1_ldu << ", val=" << mint1 << std::endl;
std::cout << "T2 Minimum found at " << mint2_x << "," << mint2_y << "( or in lVu, kuu, ldu) = " << mint2_lVu << ", " << mint2_kuu << ", " << mint2_ldu <<", val=" << mint2 << std::endl;
}
else { // Probably then use the Asimov
ldu.setVal(1);lVu.setVal(1);kuu.setVal(1);
double dnll2 = 2*spline->getVal();
mint1 = dnll2;
mint2 = dnll2;
}
TGraph *type1_0p1 = (TGraph*)gr21Dspline_tanB(spline, ldu, lVu, kuu, 1, mint1, 0.1);
TGraph *type2_0p1 = (TGraph*)gr21Dspline_tanB(spline, ldu, lVu, kuu, 2, mint2, 0.1);
type1_0p1->SetName("type1_cbma0p1");
type2_0p1->SetName("type2_cbma0p1");
type1_0p1->Write();
type2_0p1->Write();
TGraph * gr_type1 = (TGraph*)gr2contour(spline, ldu, lVu, kuu, 1, 5.99, mint1, 0, 1., 0.01, 0.001);
TGraph * gr_type2 = (TGraph*)gr2contour(spline, ldu, lVu, kuu, 2, 5.99, mint2, 0, 1., 0.01, 0.001);
gr_type1->SetName("type1");
gr_type2->SetName("type2");
gr_type1->Write();
gr_type2->Write();
//gr_type1->Draw("p");
fOut->cd();
type1_minscan->Write();
type2_minscan->Write();
gr_ldu->SetMinimum(-2.5); gr_ldu->SetMaximum(2.5);
gr_lVu->SetMinimum(0) ; gr_lVu->SetMaximum(3);
gr_kuu->SetMinimum(0) ; gr_kuu->SetMaximum(3);
gr2_ldu->SetMinimum(-2.5); gr2_ldu->SetMaximum(2.5);
gr2_lVu->SetMinimum(0) ; gr2_lVu->SetMaximum(3);
gr2_kuu->SetMinimum(0) ; gr2_kuu->SetMaximum(3);
gr_ldu->Write(); gr_lVu->Write(); gr_kuu->Write();
gr2_ldu->Write(); gr2_lVu->Write(); gr2_kuu->Write();
gr_ku->Write();
gr_kd->Write();
gr_kV->Write();
gr2_ku->Write();
gr2_kd->Write();
gr2_kV->Write();
g_FFS->Write();
gr_beta->Write();
gr_bma->Write();
std::cout << "Saved stuff to -> " << fOut->GetName() << std::endl;
fOut->Close();
}
void giPointer::update(int timeElapsed)
{
Rotation *r = getAngles(position,_pointAt);
pitch = r->pitch;
yaw = r->yaw;
}
int main(int argc, char *argv[]) {
/////VARIABLE DECLARATIONS/////
//SENSORS
mraa_i2c_context accel, gyro, mag;
float a_res, g_res, m_res;
data_t accel_data, gyro_data, mag_data, zero_rate;
int16_t temperature;
float pitch_angle, roll_angle, yaw_angle;
char *x_accel_message;
char *y_accel_message;
char *z_accel_message;
char posture_message[20];
int curr_posture;
int prev_posture = 0;
//SOCKETS AND MESSAGES
int sockfd; //Socket descriptor
int portno;
char component[256];
char endpoint[] = "127.0.0.1";
struct sockaddr_in serv_addr;
struct hostent *server; //struct containing a bunch of info
char message[256];
char serv_message[256];
int count;
int n, n1;
/////SENSOR INITIALIZATION AND SETUP
accel = accel_init();
set_accel_scale(accel, A_SCALE_2G);
set_accel_ODR(accel, A_ODR_100);
a_res = calc_accel_res(A_SCALE_2G);
gyro = gyro_init();
set_gyro_scale(gyro, G_SCALE_245DPS);
set_gyro_ODR(accel, G_ODR_190_BW_70);
g_res = calc_gyro_res(G_SCALE_245DPS);
mag = mag_init();
set_mag_scale(mag, M_SCALE_2GS);
set_mag_ODR(mag, M_ODR_125);
m_res = calc_mag_res(M_SCALE_2GS);
zero_rate = calc_gyro_offset(gyro, g_res);
portno = 2015;
//create socket
if (argc > 1) //if user supplies IP address as argument
upperbodyIP = argv[1]; //take user input as upper body IP
else
upperbodyIP = "192.168.0.30";
sockfd = socket(AF_INET, SOCK_STREAM, 0); //create a new socket
if (sockfd < 0)
error("ERROR opening socket");
server = gethostbyname(upperbodyIP); //takes a string like "www.yahoo.com", and returns a struct hostent which contains information, as IP address, address type, the length of the addresses...
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
//setup the server struct
memset((char *) &serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET; //initialize server's address
bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length);
serv_addr.sin_port = htons(portno);
//connect to server
if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0) //establish a connection to the server
error("ERROR connecting");
//WAIT FOR THE OKAY BY THE SERVER//
memset(serv_message, 0, 256);
n = read(sockfd, serv_message, 256);
if (strcmp(serv_message, "START") != 0)
error("strange response from server");
//read accel and gyro data
count = 0;
while(1) {
accel_data = read_accel(accel, a_res);
gyro_data = read_gyro(gyro, g_res);
//mag_data = read_mag(mag, m_res);
//temperature = read_temp(accel);
getAngles(accel_data, gyro_data, zero_rate, &pitch_angle, &roll_angle, &yaw_angle);
printf("is moving: %d ", isMoving(gyro_data));
printf("yaw angle: %f ", yaw_angle);
if (count == 3) {
//send posture to main edison
if (isMoving(gyro_data)==0) //if patient is stationary, calculate new posture
curr_posture = getPosture(accel_data, pitch_angle, roll_angle);
else
curr_posture = UNDEFINED; //else just use the undefined/transition case
prev_posture = curr_posture; //set new value for prev posture
memset(posture_message, 0, sizeof(char)*20);
snprintf(posture_message, 10, "%d,%f", curr_posture, yaw_angle);
//posture_message = construct_message(POS, accel_data, curr_posture);
printf("posture message: %s ", posture_message);
n = write(sockfd, posture_message, strlen(posture_message)); //write to the socket
if (n < 0)
error("ERROR writing to socket");
/*
bzero(message, 256);
printf("waiting for response ");
n1 = read(sockfd, message, 10);
if (n1 < 0)
error("ERROR reading from upper body");
printf("got response\n");
*/
count = 0;
}
printPostureString(curr_posture);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("X: %f\t Y: %f\t Z: %f\n", accel_data.x, accel_data.y, accel_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", gyro_data.x, gyro_data.y, gyro_data.z);
//printf("\tX: %f\t Y: %f\t Z: %f\t||", mag_data.x, mag_data.y, mag_data.z);
//printf("\t%ld\n", temperature);
count++;
usleep(100000);
}
return 0;
}
vector<CB_CollisionArea*> CB_CollisionArea::getConvexPolygons()
{
vector<CB_CollisionArea*> areaList;
vector<CB_Vector2D*> optimizedVectors = optimize();
uint32_t angles = getAngles(optimizedVectors);
if(angles > 0)
{
optimizedVectors = reverse(optimizedVectors);
}
if(!isConcave(optimizedVectors))
{
areaList.push_back(new CB_CollisionArea(*this));
return areaList;
}
bool repeat = true;
vector<CB_Vector2D*>::iterator it;
vector<CB_Vector2D*>::iterator itConcave;
itConcave = optimizedVectors.end();
uint32_t pSize = optimizedVectors.size();
uint32_t cit = -1;
while(repeat)
{
vector<CB_Vector2D*> tList;
uint32_t vSize = optimizedVectors.size();
if(pSize == vSize)
{
cit++;
}
else
{
pSize = vSize;
cit = 0;
}
for(uint32_t idx = 0; idx < (vSize = optimizedVectors.size()); idx++)
{
it = optimizedVectors.begin();
CB_Vector2D * curVec = *(it + (vSize + idx - 1) % vSize);
double curAngle = curVec->getAngle(**(it + idx));
if(curAngle > 0)
{
if(tList.size() > 1)
{
tList.push_back(*(it + idx));
areaList.push_back(new CB_CollisionArea(tList));
if(itConcave < it + idx)
{
idx = optimizedVectors.erase(itConcave + 1, it + idx) - optimizedVectors.begin();
}
else
{
optimizedVectors.erase(itConcave + 1);
it = optimizedVectors.erase(optimizedVectors.begin(), it + idx);
idx = 0;
}
}
tList.clear();
tList.push_back(*(it + idx));
itConcave = it + idx;
}
else if(itConcave < optimizedVectors.end())
{
tList.push_back(*(it + idx));
if(tList.size() > 2)
{
vector<CB_Vector2D*> ttList = tList;
ttList.push_back(*(it + (idx + 1) % vSize));
if(isConcave(ttList))
{
areaList.push_back(new CB_CollisionArea(tList));
tList.clear();
tList.push_back(*itConcave);
tList.push_back(*(it + idx));
if(itConcave < it + idx)
{
idx = optimizedVectors.erase(itConcave + 1, it + idx) - optimizedVectors.begin();
}
else
{
optimizedVectors.erase(itConcave + 1);
it = optimizedVectors.erase(optimizedVectors.begin(), it + idx);
idx = 0;
}
}
}
}
}
repeat = isConcave(optimizedVectors) && cit < 3;
}
if(optimizedVectors.size() > 2)
{
areaList.push_back(new CB_CollisionArea(optimizedVectors));
}
return areaList;
}
