//Get the hand regions
Node* getHandRegions(uint8_t* depth) {
Point p1 = createPoint(0,0); Point p2 = createPoint(639, 479);
Region full = createRegion(p1, p2);
Node* fullRegion = createNode(full);
//Segment the image
Node* head = segmentRegions(fullRegion, depth, PREV_H);
//Throw out small segments
head = filterRegions(head);
Node* current = head;
int handCount = 0;
while (current != NULL) {
handCount++;
current = current->next;
}
if (!handCount) {
printf("No hands detected.\n");
} else if (handCount == 1) {
printf("One hand detected.\n");
} else if (handCount == 2) {
printf("Two hands detected.\n");
} else {
printf("More than two hands detected.\n");
}
return head;
}
cpConstraint *cpSpaceSerializer::createPivotJoint(TiXmlElement *elm)
{
cpConstraint *constraint;
cpBody *a;
cpBody *b;
createBodies(elm, &a, &b);
if (elm->FirstChildElement("worldAnchor"))
{
cpVect worldPt = createPoint("worldAnchor", elm);
constraint = cpPivotJointNew(a, b, worldPt);
}
else
{
cpVect anchr1 = createPoint("anchr1", elm);
cpVect anchr2 = createPoint("anchr2", elm);
constraint = cpPivotJointNew2(a, b, anchr1, anchr2);
}
//((cpPivotJoint*)constraint)->jAcc = createPoint("jAcc", elm);
return constraint;
}
/*public*/
Geometry*
GeometryFactory::toGeometry(const Envelope* envelope) const
{
Coordinate coord;
if (envelope->isNull()) {
return createPoint();
}
if (envelope->getMinX()==envelope->getMaxX() && envelope->getMinY()==envelope->getMaxY()) {
coord.x = envelope->getMinX();
coord.y = envelope->getMinY();
return createPoint(coord);
}
CoordinateSequence *cl=CoordinateArraySequenceFactory::instance()->
create((size_t) 0, 2);
coord.x = envelope->getMinX();
coord.y = envelope->getMinY();
cl->add(coord);
coord.x = envelope->getMaxX();
coord.y = envelope->getMinY();
cl->add(coord);
coord.x = envelope->getMaxX();
coord.y = envelope->getMaxY();
cl->add(coord);
coord.x = envelope->getMinX();
coord.y = envelope->getMaxY();
cl->add(coord);
coord.x = envelope->getMinX();
coord.y = envelope->getMinY();
cl->add(coord);
Polygon *p = createPolygon(createLinearRing(cl), NULL);
return p;
}
cpShape *cpSpaceSerializer::createSegment(TiXmlElement *elm)
{
cpShape *shape;
cpBody *body = createBody(elm);
cpVect a = createPoint("a", elm);
cpVect b = createPoint("b", elm);
cpFloat radius = createValue<cpFloat>("radius", elm);
shape = cpSegmentShapeNew(body, a, b, radius);
return shape;
}
/*public*/
Point*
GeometryFactory::createPoint(const Coordinate& coordinate) const
{
if (coordinate.isNull()) {
return createPoint();
} else {
std::size_t dim = ISNAN(coordinate.z) ? 2 : 3;
CoordinateSequence *cl = coordinateListFactory->create(new vector<Coordinate>(1, coordinate), dim);
//cl->setAt(coordinate, 0);
Point *ret = createPoint(cl);
return ret;
}
}
cpBody *cpSpaceSerializer::createBody(TiXmlElement *elm)
{
TiXmlElement *bodyElm;;
cpBody *body;
CPSS_ID b_id = createValue<CPSS_ID>("body_id", elm);
BodyMap::iterator itr = _bodyMap.find(b_id);
//If it doesn't exist, try to create it
if (itr == _bodyMap.end())
{
bodyElm = elm->FirstChildElement("body");
if (bodyElm)
{
cpFloat mass = createValue<cpFloat>("mass", bodyElm);
cpFloat inertia = createValue<cpFloat>("inertia", bodyElm);
if (mass == INFINITY)
body = cpBodyNewStatic();
else
body = cpBodyNew(mass, inertia);
body->p = createPoint("p", bodyElm);
body->v = createPoint("v", bodyElm);
body->f = createPoint("f", bodyElm);
body->w = createValue<cpFloat>("w", bodyElm);
body->t = createValue<cpFloat>("t", bodyElm);
cpBodySetAngle(body, createValue<cpFloat>("a", bodyElm));
_bodyMap[b_id] = body;
if (delegate && b_id != 0)
{
if (!delegate->reading(body, b_id))
{
cpBodyFree(body);
body = NULL;
}
}
}
else
body = cpBodyNewStatic(); //Fail case, should throw or something
}
else
body = itr->second; //Else grab it
return body;
}
cpShape *cpSpaceSerializer::createPoly(TiXmlElement *elm)
{
cpShape *shape = NULL;
cpBody *body = createBody(elm);
TiXmlElement *vertsElm = elm->FirstChildElement("verts");
int numVerts = getAttribute<int>(vertsElm, "numVerts");
cpVect *verts = (cpVect*)malloc(sizeof(cpVect)*numVerts);
TiXmlElement *vertElm = vertsElm->FirstChildElement("vert");
int i;
for (i = 0; i < numVerts && vertElm; i++)
{
verts[i] = cpv(getAttribute<cpFloat>(vertElm, "x"), getAttribute<cpFloat>(vertElm, "y"));
vertElm = vertElm->NextSiblingElement("vert");
}
cpVect offset = createPoint("offset", elm);
shape = cpPolyShapeNew(body, i, verts, offset);
return shape;
}
bool WaterObjectView::load()
{
if (mIsLoaded)
{
return false;
}
if (mWaterObject->getMap() != mMapView->getMap())
{
return false;
}
if (mWaterObject->getPointCount() == 1)
{
createPoint();
}
else if (mWaterObject->getIsRiver() || (mWaterObject->getPointCount() == 2))
{
createLine();
}
else
{
createPolygon();
}
mEntity = mMapView->createEntity(mMesh->getName(), mMesh->getName());
mEntity->setUserAny(Ogre::Any(this));
mSceneNode = mMapView->createSceneNode();
mSceneNode->attachObject(mEntity);
return mIsLoaded = true;
}
struct POINT getPath(struct POINT point, int matrix[][col]){
struct POINT p = createPoint(-1, -1);
if(point.x < col && matrix[point.x+1][point.y] == 1){
p = createPoint(point.x+1, point.y);
}
else if(point.y < row && matrix[point.x][point.y+1] == 1){
p = createPoint(point.x,point.y+1);
}
else if(point.x>0 && matrix[point.x-1][point.y] == 1){
p = createPoint(point.x-1,point.y);
}
else if(point.y>0 && matrix[point.x][point.y-1] == 1){
p = createPoint(point.x,point.y-1);
}
return p;
}
void InitGL()
{
glClearColor( 1.0,1.0,1.0,1.0 );
glClearDepth(1.0);
GLenum err = glewInit();
if (GLEW_OK != err)
printf( "%s\n", glewGetErrorString(err) );
if ( GLEW_ARB_vertex_buffer_object )
printf( "VBO extersion is supported!\n");
if (GLEW_ARB_vertex_shader && GLEW_ARB_fragment_shader && GL_EXT_geometry_shader4)
printf("Ready for GLSL - vertex, fragment, and geometry units.\n");
else {
printf("Not totally ready :( \n");
}
glClearColor( 1.0,1.0,1.0,1.0);
glClearDepth(1.0);
vboVertex = -1;
vboList = -1;
if(test_mode == 1)
initVBO();
else if(test_mode == 2)
{
if(test_type == 1)
DLpoint = createPoint();
else if(test_type == 2)
DLedge = createEdge();
else if(test_type == 3)
DLtriangle = createTriangle();
}
}
/* draw figure what was chosen from the buttons */
void mouse(int button, int state, int x, int y)
{
int new_y = mapState.window_height - y;
int a;
Point* new_point ;
a = (x > PANEL_BORD_PADDING);
initFlag(x, new_y);
if (mapState.drawing_state == DRAWING_RECT && a) {
drawRectangle(x, new_y);
} else if (mapState.drawing_state == DRAWING_CIRCLE && a) {
drawCircle(x, new_y, CIRCLE_DIAMETER);
} else if (mapState.drawing_state == DRAWING_LINE && a) {
if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) {
mapState.DrawingLine = START;
mapState.new_point = createPoint(x, new_y, mapState.points_storage, mapState.marked_point);
createEdge(mapState.edges_storage, mapState.new_point, mapState.marked_point, mapState.previous_point);
mapState.previous_point = mapState.new_point;
}
draw();
}
}
// Descr: evident
// Implementation details: See gtest/samples for GTest syntax and usage
TEST(GeometryTest, GetNormal)
{
Atom atom1, atom2, atom3;
atom1.x = 3.2;
atom1.y = 4.6;
atom1.z = 0.2;
atom2.x = 7.1;
atom2.y = 1.4;
atom2.z = 10.0;
atom3.x = 0.0;
atom3.y = 0.0;
atom3.z = 0.0;
Plane testPlane = createPlane(atom1, atom2, atom3);
Point expected = createPoint(45.72, -30.58, -28.18);
Point test = getNormal(testPlane);
test.x = ((double) ((int) (test.x * 100))) / 100.0;
test.y = ((double) ((int) (test.y * 100))) / 100.0;
test.z = ((double) ((int) (test.z * 100))) / 100.0;
EXPECT_NEAR(expected.x, test.x, .01);
EXPECT_NEAR(expected.y, test.y, .01);
EXPECT_NEAR(expected.z, test.z, .01);
}
void testGetNormal()
{
cout << "Testing GetNormal" <<endl;
Atom atom1, atom2, atom3;
atom1.x = 3.2;
atom1.y = 4.6;
atom1.z = 0.2;
atom2.x = 7.1;
atom2.y = 1.4;
atom2.z = 10.0;
atom3.x = 0.0;
atom3.y = 0.0;
atom3.z = 0.0;
Plane testPlane = createPlane(atom1, atom2, atom3);
Point expected = createPoint(45.72, -30.58, -28.18);
Point test = getNormal(testPlane);
test.x = ((double) ((int) (test.x * 100))) / 100.0;
test.y = ((double) ((int) (test.y * 100))) / 100.0;
test.z = ((double) ((int) (test.z * 100))) / 100.0;
assert(test.x == expected.x);
assert(test.y == expected.y);
assert(test.z == expected.z);
cout << "Testing GetNormal Completed\n" <<endl;
}
Light* Light::clone(NodeCloneContext &context)
{
Light* lightClone = NULL;
switch (_type)
{
case DIRECTIONAL:
lightClone = createDirectional(getColor());
break;
case POINT:
lightClone = createPoint(getColor(), getRange());
break;
case SPOT:
lightClone = createSpot(getColor(), getRange(), getInnerAngle(), getOuterAngle());
break;
default:
GP_ERROR("Unsupported light type (%d).", _type);
return NULL;
}
GP_ASSERT(lightClone);
if (Node* node = context.findClonedNode(getNode()))
{
lightClone->setNode(node);
}
return lightClone;
}
// Create the board
static void createBoard(Game g, int discipline[], int dice[]) {
g->gameBoard = malloc (sizeof (struct _board));
assert(g->gameBoard != NULL);
// Create the points that exist
int x = 0;
while (x < NUM_POINTS_X) {
int y = 0;
while (y < NUM_POINTS_Y) {
if (validPoint(x, y)) {
createPoint(g, x, y);
}
y++;
}
x++;
}
// Create the regions that exist
x = 0;
int i = 0;
while (x < NUM_REGIONS_X) {
int y = 0;
while (y < NUM_REGIONS_Y) {
if (validRegion(x, y)) {
createRegion(g, x, y, i, dice[i], discipline[i]);
// Add the region to the points around it
addRegionToPoints(g, x, y);
i++;
}
y++;
}
x++;
}
}
OrderedTaskPoint*
AbstractTaskFactory::createMutatedPoint(const OrderedTaskPoint &tp,
const LegalPointType_t newtype) const
{
fixed ozsize = GetOZSize(tp.get_oz());
return createPoint(newtype, tp.GetWaypoint(), ozsize, ozsize, ozsize);
}
IntermediateTaskPoint*
AbstractTaskFactory::createIntermediate(const LegalPointType_t type,
const Waypoint &wp) const
{
if (!validIntermediateType(type))
return NULL;
return (IntermediateTaskPoint*)createPoint(type, wp);
}
OrderedTaskPoint*
AbstractTaskFactory::createPoint(const LegalPointType_t type,
const Waypoint &wp) const
{
return createPoint(type, wp,
fixed_minus_one,
fixed_minus_one,
fixed_minus_one);
}
FinishPoint*
AbstractTaskFactory::createFinish(const LegalPointType_t type,
const Waypoint &wp) const
{
if (!validFinishType(type))
return NULL;
return (FinishPoint*)createPoint(type, wp);
}
cpSpace* cpSpaceSerializer::load(cpSpace *space, const char* filename)
{
if (!_doc.LoadFile(filename))
return space;
//Grab our space
TiXmlElement *root = _doc.FirstChildElement("space");
if (!root)
return space;
_space = space;
//Initialize
_bodyMap.clear();
_shapeMap.clear();
//A body id of zero is the space's static body
_bodyMap[0] = space->staticBody;
space->iterations = createValue<int>("iterations", root);
space->gravity = createPoint("gravity", root);
space->damping = createValue<cpFloat>("damping", root);
TiXmlElement *child = root->FirstChildElement("shape");
//Read Shapes
while (child)
{
//attempt a shape
cpShape *shape = createShape(child);
if (shape)
{
//This should not happen like this, need to reflect reality -rkb
if (shape->body->m != INFINITY && !cpSpaceContainsBody(space, shape->body))
cpSpaceAddBody(space, shape->body);
cpSpaceAddShape(space, shape);
}
//Next!
child = child->NextSiblingElement("shape");
}
//Read Constraints
child = root->FirstChildElement("constraint");
while (child)
{
//else attempt a constraint
cpConstraint *constraint = createConstraint(child);
if (constraint)
cpSpaceAddConstraint(space, constraint);
child = child->NextSiblingElement("constraint");
}
return space;
}
cpConstraint *cpSpaceSerializer::createPinJoint(TiXmlElement *elm)
{
cpConstraint *constraint;
cpVect anchr1 = createPoint("anchr1", elm);
cpVect anchr2 = createPoint("anchr2", elm);
cpBody *a;
cpBody *b;
createBodies(elm, &a, &b);
constraint = cpPinJointNew(a, b, anchr1, anchr2);
((cpPinJoint*)constraint)->dist = createValue<cpFloat>("dist", elm);
//((cpPinJoint*)constraint)->jnAcc = createValue<cpFloat>("jnAcc", elm);
return constraint;
}
cpConstraint *cpSpaceSerializer::createSpringJoint(TiXmlElement *elm)
{
cpConstraint *constraint;
cpVect anchr1 = createPoint("anchr1", elm);
cpVect anchr2 = createPoint("anchr2", elm);
cpBody *a;
cpBody *b;
createBodies(elm, &a, &b);
cpFloat restLen = createValue<cpFloat>("restLength", elm);
cpFloat stiffness = createValue<cpFloat>("stiffness", elm);
cpFloat damping = createValue<cpFloat>("damping", elm);
constraint = cpDampedSpringNew(a, b, anchr1, anchr2, restLen, stiffness, damping);
return constraint;
}
cpConstraint *cpSpaceSerializer::createGrooveJoint(TiXmlElement *elm)
{
cpConstraint *constraint;
cpVect grv_a = createPoint("grv_a", elm);
cpVect grv_b = createPoint("grv_b", elm);
cpVect anchr2 = createPoint("anchr2", elm);
cpBody *a;
cpBody *b;
createBodies(elm, &a, &b);
constraint = cpGrooveJointNew(a, b, grv_a, grv_b, anchr2);
//((cpGrooveJoint*)constraint)->jAcc = createPoint("jAcc", elm);
//((cpGrooveJoint*)constraint)->jMaxLen = createValue<cpFloat>("jMaxLen", elm);
return constraint;
}
geometry_msgs::Pose convertToPose(tf::StampedTransform transform)
{
tf::Quaternion q = transform.getRotation();
tf::Point p = transform.getOrigin();
geometry_msgs::Pose pose;
pose.position = createPoint(p.x(), p.y(), p.z());
pose.orientation = createQuaternion(q.x(), q.y(), q.z(), q.w());
return pose;
}
StartPoint*
AbstractTaskFactory::createStart(const LegalPointType_t type,
const Waypoint &wp) const
{
if (!validStartType(type))
// error, invalid type!
return NULL;
return (StartPoint*)createPoint(type, wp);
}
int main()
{
Point *p1;
Point *p2;
char temp1[15];
p1 = createPoint (10, 12);
p2 = createPoint (-5, 12);
movePoint (p1, 10, 3);
dilatePoint (p2, 2);
printf ("Point 1: %s\n", pointToString (p1, temp1));
printf ("Point 2: %s\n", pointToString (p2, temp1));
free (p1);
free (p2);
}
cpConstraint *cpSpaceSerializer::createSlideJoint(TiXmlElement *elm)
{
cpConstraint *constraint;
cpVect anchr1 = createPoint("anchr1", elm);
cpVect anchr2 = createPoint("anchr2", elm);
cpBody *a;
cpBody *b;
createBodies(elm, &a, &b);
cpFloat min = createValue<cpFloat>("min", elm);
cpFloat max = createValue<cpFloat>("max", elm);
constraint = cpSlideJointNew(a, b, anchr1, anchr2, min, max);
//((cpSlideJoint*)constraint)->jnAcc = createValue<cpFloat>("jnAcc", elm);
return constraint;
}
cpShape *cpSpaceSerializer::createCircle(TiXmlElement *elm)
{
cpShape *shape;
cpBody *body = createBody(elm);
cpFloat radius = createValue<cpFloat>("radius", elm);
cpVect offset = createPoint("offset", elm);
shape = cpCircleShapeNew(body, radius, offset);
return shape;
}
void CreatePointCommandObject::move()
{
auto segments = m_startSegments;
// Locking between bounds
handleLocking();
// Creation
createPoint(segments);
// Submit
submit(std::move(segments));
}
OrderedTaskPoint*
AbstractTaskFactory::createMutatedPoint(const OrderedTaskPoint &tp,
const LegalPointType_t newtype) const
{
const ObservationZonePoint* oz = tp.get_oz();
UserSizeObservationZone soz;
ObservationZoneConstVisitor &oz_visitor = soz;
oz_visitor.Visit(*oz);
const fixed ozsize = soz.get_user_size();
return (OrderedTaskPoint*)createPoint(newtype,
tp.get_waypoint(), ozsize, ozsize, ozsize);
}