show_chess(int y, int x, char a, char b)
{
char nn1[42][7]={
"★★★",
" ★★ ",
" 將 ",
" �� ",
" 士�� ",
" �� ",
"╭─╮",
"象 .│",
" ╰ˋ",
" 車",
"╭Π╮",
"◎�龤�",
"馬 ▲",
"╭�灨�",
" ⊥⊥ ",
" △包 ",
" █ ",
"|||| ",
" ●卒",
"╰�屣�",
" ╱╲ ",
"★★★",
" ★★ ",
" 帥 ",
" �� ",
" 仕�� ",
" �� ",
"╭─╮",
"相 .│",
" ╰ˋ",
" 硨",
"╭Π╮",
"◎�龤�",
"傌 ▲",
"╭�灨�",
" ⊥⊥ ",
" △炮 ",
" █ ",
"|||| ",
" ●兵",
"╰�屣�",
" ╱╲ "};
y=y*24+20;
x=x*4+2;
if(b)
{
char p,i;
p=(red(a)*7 + (a%64)/2-1)*3;
for(i=0; i<3; i++) printtt(x+i,y,"3%dm%s",red(a),nn1[i+p]);
}
else
{
char *ptr;
ptr = nn + red(a)*14 + a%64 - a%2;
printtt(x ,y,"3%dm%s", red(a), a?"╭─╮":" ");
printtt(x+1,y,"3%dm%s%c%c%s",
red(a), a?"│":" ", *ptr, *(ptr+1), a?"│":" ");
printtt(x+2,y,"3%dm%s", red(a), a?"╰─╯":" ");
}
refresh();
}
void Display::draw()
{
std::vector<Particle*> particles = theFluid->getParticles();
int w = camera->getWidth();
int h = camera->getHeight();
//BEGIN render from light
glUseProgram(shadowShaderProgram);
glBindFramebuffer(GL_FRAMEBUFFER, fbo);
// Clear previous frame values
glClear( GL_DEPTH_BUFFER_BIT);
//Disable color rendering, we only want to write to the Z-Buffer
glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
glCullFace( GL_FRONT );
camera->setViewport(SHADOW_MAP_SIZE, SHADOW_MAP_SIZE);
glViewport(0,0,SHADOW_MAP_SIZE,SHADOW_MAP_SIZE);
vec3 pos = camera->getPos();
camera->setPos( vec3( lightPos.x, lightPos.y, lightPos.z ) );
mat4 cmat = camera->getMat4();
glUniformMatrix4fv(u_shadowProjMatrixLocation, 1, GL_FALSE, &cmat[0][0]);
if( pos.y > 0 )
world->draw( shadowPositionLocation, colorLocation, normalLocation, u_shadowModelMatrixLocation );
//TODO: draw particles
World::Shape * particle = new World::Cube();
for (unsigned int i = 0; i < particles.size(); i++)
{
int pidx = particles.at( i )->getIndex();
if( flags & pidx )
{
particle->clearMat();
particle->translate(particles.at(i)->getPosition());
particle->scale( vec3( 0.04 ) );
particle->draw( shadowPositionLocation, colorLocation, normalLocation, u_shadowModelMatrixLocation );
}
}
//END render from light
//BEGIN render from camera
glUseProgram(shaderProgram);
glBindFramebuffer(GL_FRAMEBUFFER, 0);
glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glUniform1i( u_shadowMapLocation, 7 );
glActiveTexture(GL_TEXTURE7);
glBindTexture(GL_TEXTURE_2D,depthTexture);
float bias[16] = { 0.5, 0.0, 0.0, 0.0,
0.0, 0.5, 0.0, 0.0,
0.0, 0.0, 0.5, 0.0,
0.5, 0.5, 0.5, 1.0 };
mat4 cameraMatrix = make_mat4( bias ) * cmat;
glUniformMatrix4fv(u_shadowBiasMatrixLocation, 1, GL_FALSE, &cameraMatrix[0][0]);
camera->setViewport(w, h);
camera->setPos( pos );
glViewport(0,0,w,h);
glCullFace( GL_BACK );
if( pos.y > 0 )
world->draw( positionLocation, colorLocation, normalLocation, u_modelMatrixLocation );
//TODO: draw particles
vec3 red( 1,0,0 );
vec3 blue( 0,0,1 );
for (unsigned int i = 0; i < particles.size(); i++)
{
int pidx = particles.at( i )->getIndex();
if( flags & pidx )
{
particle->clearMat();
particle->translate(particles.at(i)->getPosition());
particle->scale( vec3( 0.04 ) );
if( flags & DFLAG_VEL )
{
particle->setColor( 0.1f+glm::clamp( particles.at(i)->getVelocity()*particles.at(i)->getVelocity()/5.0f, vec3(0.0), vec3(1.0) ) );
}
else if( flags & DFLAG_TEMP )
{
float alpha = ( particles.at( i )->getTemp() - 5 ) / 10;
particle->setColor(alpha*red + (1-alpha)*blue);
}
else
{
particle->setColor( colorMap[pidx] );
}
particle->draw( positionLocation, colorLocation, normalLocation, u_modelMatrixLocation );
}
}
delete particle;
//END render from camera
glBindTexture(GL_TEXTURE_2D,0);
}
//----------------------------------------------------------------------------
void IntersectConvexPolyhedra::CreateScene ()
{
mScene = new0 Node();
mMotionObject = mScene;
VertexFormat* vformat = VertexFormat::Create(2,
VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_COLOR, VertexFormat::AT_FLOAT3, 0);
int vstride = vformat->GetStride();
// Attach a dummy intersection mesh. If the intersection is nonempty,
// the Culling flag will be modified to CULL_DYNAMIC. The intersection
// is drawn as a solid.
mMeshIntersection = StandardMesh(vformat).Tetrahedron();
VertexBufferAccessor vba(mMeshIntersection);
Float3 green(0.0f, 1.0f, 0.0f);
int i, j;
for (i = 0; i < vba.GetNumVertices(); ++i)
{
vba.Color<Float3>(0, i) = green;
}
mMeshIntersection->SetEffectInstance(
VertexColor3Effect::CreateUniqueInstance());
mMeshIntersection->Culling = Spatial::CULL_ALWAYS;
mScene->AttachChild(mMeshIntersection);
// The first polyhedron is an ellipsoid.
ConvexPolyhedronf::CreateEggShape(Vector3f::ZERO, 1.0f, 1.0f, 2.0f, 2.0f,
4.0f, 4.0f, 3, mWorldPoly0);
// Build the corresponding mesh.
int numVertices = mWorldPoly0.GetNumVertices();
int numTriangles = mWorldPoly0.GetNumTriangles();
int numIndices = 3*numTriangles;
VertexBuffer* vbuffer = new0 VertexBuffer(numVertices, vstride);
IndexBuffer* ibuffer = new0 IndexBuffer(numIndices, sizeof(int));
Float3 red(1.0f, 0.0f, 0.0f);
vba.ApplyTo(vformat, vbuffer);
for (i = 0; i < numVertices; ++i)
{
vba.Position<Vector3f>(i) = mWorldPoly0.Point(i);
vba.Color<Float3>(0,i) = red;
}
int* indices = (int*)ibuffer->GetData();
for (i = 0; i < numTriangles; ++i)
{
const MTTriangle& triangle = mWorldPoly0.GetTriangle(i);
for (j = 0; j < 3; ++j)
{
indices[3*i + j] = mWorldPoly0.GetVLabel(triangle.GetVertex(j));
}
}
mMeshPoly0 = new0 TriMesh(vformat, vbuffer, ibuffer);
VisualEffectInstance* instance =
VertexColor3Effect::CreateUniqueInstance();
instance->GetEffect()->GetWireState(0, 0)->Enabled = true;
mMeshPoly0->SetEffectInstance(instance);
mMeshPoly0->LocalTransform.SetTranslate(APoint(0.0f, 2.0f, 0.0f));
mScene->AttachChild(mMeshPoly0);
// The second polyhedron is egg shaped.
ConvexPolyhedronf::CreateEggShape(Vector3f::ZERO, 2.0f, 2.0f, 4.0f, 4.0f,
5.0f, 3.0f, 4, mWorldPoly1);
// Build the corresponding mesh.
numVertices = mWorldPoly1.GetNumVertices();
numTriangles = mWorldPoly1.GetNumTriangles();
numIndices = 3*numTriangles;
vbuffer = new0 VertexBuffer(numVertices, vstride);
ibuffer = new0 IndexBuffer(numIndices, sizeof(int));
Float3 blue(0.0f, 0.0f, 1.0f);
vba.ApplyTo(vformat, vbuffer);
for (i = 0; i < numVertices; ++i)
{
vba.Position<Vector3f>(i) = mWorldPoly1.Point(i);
vba.Color<Float3>(0, i) = blue;
}
indices = (int*)ibuffer->GetData();
for (i = 0; i < numTriangles; ++i)
{
const MTTriangle& triangle = mWorldPoly1.GetTriangle(i);
for (j = 0; j < 3; ++j)
{
indices[3*i + j] = mWorldPoly1.GetVLabel(triangle.GetVertex(j));
}
}
mMeshPoly1 = new0 TriMesh(vformat, vbuffer, ibuffer);
instance = VertexColor3Effect::CreateUniqueInstance();
instance->GetEffect()->GetWireState(0, 0)->Enabled = true;
mMeshPoly1->SetEffectInstance(instance);
mMeshPoly1->LocalTransform.SetTranslate(APoint(0.0f, -2.0f, 0.0f));
mScene->AttachChild(mMeshPoly1);
ComputeIntersection();
}
int
main (int argc, char ** argv)
{
if (argc < 3)
{
pcl::console::print_info ("Syntax is: %s source target <options>\n", argv[0]);
pcl::console::print_info (" where options are:\n");
pcl::console::print_info (" -i min_sample_dist,max_dist,nr_iters ................ Compute initial alignment\n");
pcl::console::print_info (" -r max_dist,rejection_thresh,tform_eps,max_iters ............. Refine alignment\n");
pcl::console::print_info (" -s output.pcd ........................... Save the registered and merged clouds\n");
pcl::console::print_info ("Note: The inputs (source and target) must be specified without the .pcd extension\n");
return (1);
}
// Load the points
PointCloudPtr src_points = loadPoints (argv[1]);
PointCloudPtr tgt_points = loadPoints (argv[2]);
Eigen::Matrix4f tform = Eigen::Matrix4f::Identity ();
// Compute the intial alignment
double min_sample_dist, max_correspondence_dist, nr_iters;
bool compute_intial_alignment =
pcl::console::parse_3x_arguments (argc, argv, "-i", min_sample_dist, max_correspondence_dist, nr_iters) > 0;
if (compute_intial_alignment)
{
// Load the keypoints and local descriptors
PointCloudPtr src_keypoints = loadKeypoints (argv[1]);
LocalDescriptorsPtr src_descriptors = loadLocalDescriptors (argv[1]);
PointCloudPtr tgt_keypoints = loadKeypoints (argv[2]);
LocalDescriptorsPtr tgt_descriptors = loadLocalDescriptors (argv[2]);
// Find the transform that roughly aligns the points
tform = computeInitialAlignment (src_keypoints, src_descriptors, tgt_keypoints, tgt_descriptors,
min_sample_dist, max_correspondence_dist, nr_iters);
pcl::console::print_info ("Computed initial alignment\n");
}
// Refine the initial alignment
std::string params_string;
bool refine_alignment = pcl::console::parse_argument (argc, argv, "-r", params_string) > 0;
if (refine_alignment)
{
std::vector<std::string> tokens;
boost::split (tokens, params_string, boost::is_any_of (","), boost::token_compress_on);
assert (tokens.size () == 4);
float max_correspondence_distance = atof(tokens[0].c_str ());
float outlier_rejection_threshold = atof(tokens[1].c_str ());
float transformation_epsilon = atoi(tokens[2].c_str ());
int max_iterations = atoi(tokens[3].c_str ());
tform = refineAlignment (src_points, tgt_points, tform, max_correspondence_distance,
outlier_rejection_threshold, transformation_epsilon, max_iterations);
pcl::console::print_info ("Refined alignment\n");
}
// Transform the source point to align them with the target points
pcl::transformPointCloud (*src_points, *src_points, tform);
// Save output
std::string filename;
bool save_output = pcl::console::parse_argument (argc, argv, "-s", filename) > 0;
if (save_output)
{
// Merge the two clouds
(*src_points) += (*tgt_points);
// Save the result
pcl::io::savePCDFile (filename, *src_points);
pcl::console::print_info ("Saved registered clouds as %s\n", filename.c_str ());
}
// Or visualize the result
else
{
pcl::console::print_info ("Starting visualizer... Close window to exit\n");
pcl::visualization::PCLVisualizer vis;
pcl::visualization::PointCloudColorHandlerCustom<PointT> red (src_points, 255, 0, 0);
vis.addPointCloud (src_points, red, "src_points");
pcl::visualization::PointCloudColorHandlerCustom<PointT> yellow (tgt_points, 255, 255, 0);
vis.addPointCloud (tgt_points, yellow, "tgt_points");
vis.resetCamera ();
vis.spin ();
}
return (0);
}
int
place_item (int x, int y, short type)
{
int group;
int size;
group = get_group_of_type(type);
if (group < 0) return -1;
size = main_groups[group].size;
/* You can't build because credit not available. */
if (no_credit_build (group) != 0) {
return -1;
}
/* Not enough slots in the substation array */
switch (group) {
case GROUP_SUBSTATION:
case GROUP_WINDMILL:
{
if (add_a_substation (x, y) == 0)
return -3;
} break;
case GROUP_PORT:
{
if (is_real_river (x + 4, y) != 1
|| is_real_river (x + 4, y + 1) != 1
|| is_real_river (x + 4, y + 2) != 1
|| is_real_river (x + 4, y + 3) != 1) {
return -2;
}
} break;
case GROUP_COMMUNE:
{
numof_communes++;
} break;
case GROUP_MARKET:
{
/* Test for enough slots in the market array */
if (add_a_market (x, y) == 0)
return -3;
MP_INFO(x,y).flags += (FLAG_MB_FOOD | FLAG_MB_JOBS
| FLAG_MB_COAL | FLAG_MB_ORE | FLAG_MB_STEEL
| FLAG_MB_GOODS | FLAG_MS_FOOD | FLAG_MS_JOBS
| FLAG_MS_COAL | FLAG_MS_GOODS | FLAG_MS_ORE
| FLAG_MS_STEEL);
} break;
case GROUP_TIP:
{
/* Don't build a tip if there has already been one. If we succeed,
mark the spot permanently by "doubling" the ore reserve */
int i,j;
int prev_tip = 0;
for (i=0;i<3;i++) {
for (j=0;j<3;j++) {
if (MP_INFO(x+i,y+j).ore_reserve > ORE_RESERVE) {
prev_tip = 1;
break;
}
}
}
if (prev_tip) {
dialog_box(red(12),3,
0,0,_("You can't build a tip here"),
0,0,_("This area was once a landfill"),
2,' ',_("OK"));
return -4;
} else {
for (i=0;i<3;i++) {
for (j=0;j<3;j++) {
MP_INFO(x+i,y+j).ore_reserve = ORE_RESERVE * 2;
}
}
}
} break;
case GROUP_OREMINE:
{
/* Don't allow new mines on old mines or old tips */
/* GCS: mines over old mines is OK if there is enough remaining
ore, as is the case when there is partial overlap. */
int i,j;
int prev_tip = 0;
int total_ore = 0;
for (i=0;i<3;i++) {
for (j=0;j<3;j++) {
total_ore += MP_INFO(x+i,y+j).ore_reserve;
if (MP_INFO(x+i,y+j).ore_reserve > ORE_RESERVE) {
prev_tip = 1;
break;
}
}
}
if (prev_tip) {
dialog_box(red(12),3,
0,0,_("You can't build a mine here"),
0,0,_("This area was once a landfill"),
2,' ',_("OK"));
return -4;
}
if (total_ore < MIN_ORE_RESERVE_FOR_MINE) {
dialog_box(red(12),3,
0,0,_("You can't build a mine here"),
0,0,_("There is no ore left at this site"),
2,' ',_("OK"));
return -4;
}
}
} /* end case */
/* Store last_built for refund on "mistakes" */
last_built_x = x;
last_built_y = y;
/* Make sure that the correct windmill graphic shows up */
if (group == GROUP_WINDMILL) {
if (tech_level > MODERN_WINDMILL_TECH) {
type = CST_WINDMILL_1_R;
} else {
type = CST_WINDMILL_1_W;
}
}
if (group == GROUP_SOLAR_POWER || group == GROUP_WINDMILL) {
MP_INFO(x,y).int_2 = tech_level;
let_one_through = 1;
}
else if (group == GROUP_RECYCLE || group == GROUP_COAL_POWER)
MP_INFO(x,y).int_4 = tech_level;
else if (group == GROUP_ORGANIC_FARM)
MP_INFO(x,y).int_1 = tech_level;
else if (group == GROUP_TRACK
|| group == GROUP_ROAD
|| group == GROUP_RAIL)
MP_INFO(x,y).flags |= FLAG_IS_TRANSPORT;
else if (group == GROUP_COALMINE
|| group == GROUP_OREMINE)
let_one_through = 1;
set_mappoint (x, y, type);
update_tech_dep (x, y);
if (group == GROUP_RIVER)
connect_rivers ();
connect_transport (x-2,y-2,x+size+1,y+size+1);
adjust_money(-selected_module_cost);
map_power_grid();
return 0;
}
int main(int argc, char **argv)
{
cout << "Starting" << endl;
double point[2];
double * current = new double[9];
double * best = new double[9];
double * init = new double[9];
double ncc;
double bestncc = -2;
double first;
double scale = 1;
//int position = 0;
//int direction = 1;
bool optimize = true;
cout << "Starting" << endl;
vector<PixelLoc> interior;
for(int i=9; i<=23; ++i){
for(int j=9; j<=23; ++j){
PixelLoc point(i, j);
interior.push_back(point);
}
}
cout << "Creating Images";
Image myimg("test-initial.ppm");
Image myimgOther("test-final.ppm");
cout << "Images created";
for(int i=0;i<9;++i){
init[i] = current[i] = best[i] = 0;
}
init[8] = current[8] = best[8] = 1;
init[0] = current[0] = best[0] = 1;
init[4] = current[4] = best[4] = 1;
cout << "initial homography: " << endl;
for (int i = 0; i < 9; i++){
cout << init[i] << " ";
}
Color red(255,0,0);
Color blue(0,0,100);
Image imgInitial = myimg;
Image src = myimgOther;
for(unsigned int i=0; i<interior.size(); ++i){
homography(interior[i].x, interior[i].y, current, point);
PixelLoc loc((int)point[0], (int)point[1]);
if(inImage(&imgInitial,loc)){
imgInitial.setPixel(loc,blue);
}
}
imgInitial.print("initial.ppm");
for(unsigned int i=0; i<interior.size(); ++i){
if(inImage(&src,interior[i])){
src.setPixel(interior[i],blue);
}
}
src.print("src.ppm");
cout << endl;
if(optimize){
Optimize (scale, first, ncc, bestncc, &interior, init, current, best, &myimg, &myimgOther);
}
cout << "First: " << first << " Best: " << bestncc << endl;
cout << "homography: ";
for(int i=0;i<9;++i){
cout << current[i] << " ";
}
cout << endl;
Image imgFinal = myimg;
printHomographyTile(&imgFinal,&imgInitial,interior,best);
system("/home/mscs/bin/show src.ppm initial.ppm final.ppm");
}
/// Changes a btManifoldPoint collision normal to the normal from the mesh.
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* colObj0,const btCollisionObject* colObj1, int partId0, int index0, int normalAdjustFlags)
{
//btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
return;
btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape();
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
if (!triangleInfoMapPtr)
return;
int hash = btGetHash(partId0,index0);
btTriangleInfo* info = triangleInfoMapPtr->find(hash);
if (!info)
return;
btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0->getCollisionShape());
btVector3 v0,v1,v2;
tri_shape->getVertex(0,v0);
tri_shape->getVertex(1,v1);
tri_shape->getVertex(2,v2);
btVector3 center = (v0+v1+v2)*btScalar(1./3.);
btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
btVector3 tri_normal;
tri_shape->calcNormal(tri_normal);
//btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
btVector3 nearest;
btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);
btVector3 contact = cp.m_localPointB;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
const btTransform& tr = colObj0->getWorldTransform();
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
bool isNearEdge = false;
int numConcaveEdgeHits = 0;
int numConvexEdgeHits = 0;
btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
localContactNormalOnB.normalize();//is this necessary?
if ((info->m_edgeV0V1Angle)< SIMD_2_PI)
{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif
btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
{
btVector3 edge(v0-v1);
isNearEdge = true;
if (info->m_edgeV0V1Angle==btScalar(0))
{
numConcaveEdgeHits++;
} else
{
bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 nA = swapFactor * tri_normal;
btQuaternion orn(edge,info->m_edgeV0V1Angle);
btVector3 computedNormalB = quatRotate(orn,tri_normal);
if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
computedNormalB*=-1;
btVector3 nB = swapFactor*computedNormalB;
btScalar NdotA = localContactNormalOnB.dot(nA);
btScalar NdotB = localContactNormalOnB.dot(nB);
bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
#ifdef DEBUG_INTERNAL_EDGE
{
btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
}
#endif //DEBUG_INTERNAL_EDGE
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
btVector3 clampedLocalNormal;
bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
{
btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal. (what about cp.m_distance1?)
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
btNearestPointInLineSegment(contact,v1,v2,nearest);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
if ((info->m_edgeV1V2Angle)< SIMD_2_PI)
{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
{
isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 edge(v1-v2);
isNearEdge = true;
if (info->m_edgeV1V2Angle == btScalar(0))
{
numConcaveEdgeHits++;
} else
{
bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0;
btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 nA = swapFactor * tri_normal;
btQuaternion orn(edge,info->m_edgeV1V2Angle);
btVector3 computedNormalB = quatRotate(orn,tri_normal);
if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
computedNormalB*=-1;
btVector3 nB = swapFactor*computedNormalB;
#ifdef DEBUG_INTERNAL_EDGE
{
btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
}
#endif //DEBUG_INTERNAL_EDGE
btScalar NdotA = localContactNormalOnB.dot(nA);
btScalar NdotB = localContactNormalOnB.dot(nB);
bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
btVector3 clampedLocalNormal;
bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
{
btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
btNearestPointInLineSegment(contact,v2,v0,nearest);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
if ((info->m_edgeV2V0Angle)< SIMD_2_PI)
{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
{
isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 edge(v2-v0);
if (info->m_edgeV2V0Angle==btScalar(0))
{
numConcaveEdgeHits++;
} else
{
bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0;
btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 nA = swapFactor * tri_normal;
btQuaternion orn(edge,info->m_edgeV2V0Angle);
btVector3 computedNormalB = quatRotate(orn,tri_normal);
if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
computedNormalB*=-1;
btVector3 nB = swapFactor*computedNormalB;
#ifdef DEBUG_INTERNAL_EDGE
{
btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
}
#endif //DEBUG_INTERNAL_EDGE
btScalar NdotA = localContactNormalOnB.dot(nA);
btScalar NdotB = localContactNormalOnB.dot(nB);
bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
// printf("hitting convex edge\n");
btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
btVector3 clampedLocalNormal;
bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
{
btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
#ifdef DEBUG_INTERNAL_EDGE
{
btVector3 color(0,1,1);
btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color);
}
#endif //DEBUG_INTERNAL_EDGE
if (isNearEdge)
{
if (numConcaveEdgeHits>0)
{
if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0)
{
//fix tri_normal so it pointing the same direction as the current local contact normal
if (tri_normal.dot(localContactNormalOnB) < 0)
{
tri_normal *= -1;
}
cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal;
} else
{
//modify the normal to be the triangle normal (or backfacing normal)
cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing);
}
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);
}
}
}
int main(int argc, char* argv[]) {
// compiler will whine about it being deprecated, but taking it out
// blows things up
// used for GIO
g_type_init();
g_log_set_always_fatal(G_LOG_LEVEL_ERROR);
g_log_set_handler(G_LOG_DOMAIN, G_LOG_LEVEL_DEBUG |
G_LOG_LEVEL_ERROR |
G_LOG_LEVEL_WARNING |
G_LOG_LEVEL_MESSAGE |
G_LOG_FLAG_RECURSION |
G_LOG_FLAG_FATAL, erln8_log, NULL);
homedir = g_getenv("ERLN8_HOME");
if(homedir == NULL) {
homedir = g_get_home_dir();
} else {
// builds will fail if ERLN8_HOME is not an absolute path
if(homedir[0] != '/') {
g_error("ERLN8_HOME must be an absolute path\n");
}
}
g_debug("home directory = %s\n", homedir);
gchar* basename = g_path_get_basename(argv[0]);
g_debug("basename = %s\n", basename);
if((!strcmp(basename, "erln8")) || (!strcmp(basename, "./erln8"))) {
erln8(argc, argv);
g_free(basename);
} else {
gchar* erl = which_erlang();
if(erl == NULL) {
g_message("Can't find an " ERLN8_CONFIG_FILE " file to use\n");
g_error("No " ERLN8_CONFIG_FILE " file\n");
}
GHashTable* erlangs = get_erlangs();
GHashTable* runtime_options = get_erln8();
gchar* path = g_hash_table_lookup(erlangs, erl);
if(path == NULL) {
g_hash_table_destroy(erlangs);
g_hash_table_destroy(runtime_options);
g_error("Version of Erlang (%s) isn't configured in erln8\n",
erl);
}
gchar* use_color = (gchar*)g_hash_table_lookup(runtime_options, "color");
if(g_strcmp0(use_color, "true") == 0) {
opt_color = TRUE;
} else {
opt_color = FALSE;
}
gchar* use_banner = (gchar*)g_hash_table_lookup(runtime_options, "banner");
if(g_strcmp0(use_banner, "true") == 0) {
opt_banner = TRUE;
} else {
opt_banner = FALSE;
}
gchar* s = get_bin(erl, basename);
g_debug("%s\n",s);
gboolean result = g_file_test(s,
G_FILE_TEST_EXISTS |
G_FILE_TEST_IS_REGULAR);
g_free(basename);
if(!result) {
g_hash_table_destroy(erlangs);
g_hash_table_destroy(runtime_options);
g_free(s);
g_error("Can't run %s, check to see if the file exists\n", s);
}
if(opt_banner) {
printf("%s", red());
printf("erln8: %s", blue());
printf("using Erlang %s", path);
printf("%s\n", color_reset());
}
g_hash_table_destroy(erlangs);
g_hash_table_destroy(runtime_options);
// can't free s
execv(s, argv);
}
return 0;
}
//----------------------------------------------------------------------------
Node* RoughPlaneSolidBox::CreateBox ()
{
mBox = new0 Node();
float xExtent = (float)mModule.XLocExt;
float yExtent = (float)mModule.YLocExt;
float zExtent = (float)mModule.ZLocExt;
VertexFormat* vformat = VertexFormat::Create(2,
VertexFormat::AU_POSITION, VertexFormat::AT_FLOAT3, 0,
VertexFormat::AU_COLOR, VertexFormat::AT_FLOAT3, 0);
StandardMesh sm(vformat);
VertexColor3Effect* effect = new0 VertexColor3Effect();
VertexBufferAccessor vba;
Transform transform;
TriMesh* face;
int i;
// +z face
Float3 red(1.0f, 0.0f, 0.0f);
transform.SetTranslate(APoint(0.0f, 0.0f, zExtent));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, xExtent, yExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = red;
vba.Color<Float3>(0, 1) = red;
vba.Color<Float3>(0, 2) = red;
vba.Color<Float3>(0, 3) = red;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
// -z face
Float3 darkRed(0.5f, 0.0f, 0.0f);
transform.SetTranslate(APoint(0.0f, 0.0f, -zExtent));
transform.SetRotate(HMatrix(AVector::UNIT_Y, AVector::UNIT_X,
-AVector::UNIT_Z, APoint::ORIGIN, true));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, yExtent, xExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = darkRed;
vba.Color<Float3>(0, 1) = darkRed;
vba.Color<Float3>(0, 2) = darkRed;
vba.Color<Float3>(0, 3) = darkRed;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
// +y face
Float3 green(0.0f, 1.0f, 0.0f);
transform.SetTranslate(APoint(0.0f, yExtent, 0.0f));
transform.SetRotate(HMatrix(AVector::UNIT_Z, AVector::UNIT_X,
AVector::UNIT_Y, APoint::ORIGIN, true));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, zExtent, xExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = green;
vba.Color<Float3>(0, 1) = green;
vba.Color<Float3>(0, 2) = green;
vba.Color<Float3>(0, 3) = green;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
// -y face
Float3 darkGreen(0.0f, 1.0f, 0.0f);
transform.SetTranslate(APoint(0.0f, -yExtent, 0.0f));
transform.SetRotate(HMatrix(AVector::UNIT_X, AVector::UNIT_Z,
-AVector::UNIT_Y, APoint::ORIGIN, true));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, xExtent, zExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = darkGreen;
vba.Color<Float3>(0, 1) = darkGreen;
vba.Color<Float3>(0, 2) = darkGreen;
vba.Color<Float3>(0, 3) = darkGreen;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
// +x face
Float3 blue(0.0f, 0.0f, 1.0f);
transform.SetTranslate(APoint(xExtent, 0.0f, 0.0f));
transform.SetRotate(HMatrix(AVector::UNIT_Y, AVector::UNIT_Z,
AVector::UNIT_X, APoint::ORIGIN, true));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, yExtent, zExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = blue;
vba.Color<Float3>(0, 1) = blue;
vba.Color<Float3>(0, 2) = blue;
vba.Color<Float3>(0, 3) = blue;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
// -x face
Float3 darkBlue(0.0f, 0.0f, 1.0f);
transform.SetTranslate(APoint(-xExtent, 0.0f, 0.0f));
transform.SetRotate(HMatrix(AVector::UNIT_Z, AVector::UNIT_Y,
-AVector::UNIT_X, APoint::ORIGIN, true));
sm.SetTransform(transform);
face = sm.Rectangle(2, 2, zExtent, yExtent);
vba.ApplyTo(face);
for (i = 0; i < 4; ++i)
{
vba.Color<Float3>(0, 0) = darkBlue;
vba.Color<Float3>(0, 1) = darkBlue;
vba.Color<Float3>(0, 2) = darkBlue;
vba.Color<Float3>(0, 3) = darkBlue;
}
face->SetEffectInstance(effect->CreateInstance());
mBox->AttachChild(face);
MoveBox();
return mBox;
}
inline quint8 qRed( Colour16 _colour16 ) {
return red(_colour16) * 8;
}
void Viewer::display()
{
int changedIndex;
openni::Status rc = openni::OpenNI::waitForAnyStream(m_stream, 2, &changedIndex);
if (rc != openni::STATUS_OK)
{
printf("Wait failed\n");
return;
}
if (changedIndex == 0 && !first)
{
m_depth.readFrame(&m_depthFrame);
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
gluLookAt(cx, 1.0f, cz,
cx + lx, 1.0f, cz + lz,
0.0f, 1.0f, 0.0f);
memset(m_inPlane, false, m_width * m_height * sizeof(bool));
const openni::DepthPixel* pDepthRow = (const openni::DepthPixel*)m_depthFrame.getData();
int rowSize = m_depthFrame.getStrideInBytes() / sizeof(openni::DepthPixel);
int cnt = 0;
for (int y = 0; y < m_depthFrame.getHeight(); y += 2)
{
const openni::DepthPixel* pDepth = pDepthRow;
for (int x = 0; x < m_depthFrame.getWidth(); x += 2, pDepth += 2)
{
if (*pDepth != 0)
{
float wx, wy, wz;
openni::CoordinateConverter::convertDepthToWorld(m_depth, x, y, *pDepth, &wx, &wy, &wz);
//depthToWorld(x, y, *pDepth, &wx, &wy, &wz);
m_pointCloud[cnt][0] = wx;
m_pointCloud[cnt][1] = wy;
m_pointCloud[cnt][2] = wz;
m_pixelToCloud[y * m_width + x] = cnt;
cnt++;
}
else
m_pixelToCloud[y * m_width + x] = -1;
}
pDepthRow += rowSize * 2;
}
float intersect;
float normalX, normalY, normalZ, d;
m_ransac->MarkPointsInBestFitPlane(m_pointCloud, cnt, m_inPlane, &normalX, &normalY, &normalZ, &d, &intersect);
pDepthRow = (const openni::DepthPixel*)m_depthFrame.getData();
rowSize = m_depthFrame.getStrideInBytes() / sizeof(openni::DepthPixel);
for (int y = 0; y < m_depthFrame.getHeight(); y += 2)
{
for (int x = 0; x < m_depthFrame.getWidth(); x += 2)
{
m_binaryImage[y / 2][x / 2] = m_pixelToCloud[y * m_width + x] != -1 ? !m_inPlane[m_pixelToCloud[y * m_width + x]] : false;
}
}
int blobs = m_blobExtractor->ExtractBlobs(m_binaryImage, m_blobs);
vector<float*> blobPoints[blobs];
vector<openni::RGB888Pixel> blobColors[blobs];
for (int y = 0; y < m_depthFrame.getHeight(); y += 2)
{
for (int x = 0; x < m_depthFrame.getWidth(); x += 2)
{
if (m_pixelToCloud[y * m_width + x] != -1)
{
int i = m_pixelToCloud[y * m_width + x];
if (m_blobs[y / 2][x / 2] == 0)
{
}
else
{
openni::RGB888Pixel pixel = m_colorData[y * m_width + x];
blobColors[m_blobs[y / 2][x / 2] - 1].push_back(pixel);
glColor3f(pixel.r / 255.0, pixel.g / 255.0, pixel.b / 255.0);
glBegin(GL_POINTS);
glVertex3f(m_pointCloud[i][0], m_pointCloud[i][1], -m_pointCloud[i][2]);
glEnd();
blobPoints[m_blobs[y / 2][x / 2] - 1].push_back(m_pointCloud[i]);
}
}
}
}
float maxPercentRed = -1;
float maxPercentBlue = -1;
int blueBlob;
int redBlob;
for (int i = 0; i < blobs; i++)
{
int numBlue = 0, numRed = 0;
for (vector<openni::RGB888Pixel>::iterator it = blobColors[i].begin(); it < blobColors[i].end(); it++)
{
openni::RGB888Pixel pixel = *it;
if (pixel.b > 100 && pixel.b - pixel.r > 0 && pixel.b - pixel.g > 0)
{
numBlue++;
}
if (pixel.r > 100 && pixel.r - pixel.g > 50 && pixel.r - pixel.b > 50)
{
numRed++;
}
}
if ((float)numBlue / blobColors[i].size() > maxPercentBlue)
{
maxPercentBlue = (float)numBlue / blobColors[i].size();
blueBlob = i;
}
if ((float)numRed / blobColors[i].size() > maxPercentRed)
{
maxPercentRed = (float)numRed / blobColors[i].size();
redBlob = i;
}
}
float redX, redY, redZ;
float blueX, blueY, blueZ;
if (blobs > 0)
{
pointCloudCenter(blobPoints[blueBlob], blobColors[blueBlob], &blueX, &blueY, &blueZ);
glPointSize( 20.0 );
glColor3f(0, 0, 1);
glBegin(GL_POINTS);
glVertex3f(blueX, blueY, -blueZ);
glEnd();
pointCloudCenter(blobPoints[redBlob], blobColors[redBlob], &redX, &redY, &redZ);
glColor3f(1, 0, 0);
glBegin(GL_POINTS);
glVertex3f(redX, redY, -redZ);
glEnd();
glPointSize( 2.0 );
}
float originX = -d * normalX;
float originY = -d * normalY;
float originZ = -d * normalZ;
glColor3f(1, 1, 0);
// glBegin(GL_LINES);
// glVertex3f(0,0,-intersect);
// glVertex3f(normalX * 100, normalY * 100, -(intersect + normalZ * 100));
// glEnd();
// glBegin(GL_LINES);
// glVertex3f(0,0,0);
// glVertex3f(0,0,-intersect);
// glEnd();
// glBegin(GL_LINES);
// glVertex3f(originX, originY, -originZ);
// glVertex3f(0, 0, -intersect);
// glEnd();
float yx = -normalX;
float yy = -normalY;
float yz = -normalZ;
float mag = sqrt(originX * originX + originY * originY + (originZ - intersect) * (originZ - intersect));
float zx = -originX / mag;
float zy = -originY / mag;
float zz = (intersect - originZ) / mag;
float xx = yy * zz - yz * zy;
float xy = yz * zx - yx * zz;
float xz = yx * zy - yy * zx;
// originX = originY = 0;
// originZ = intersect;
glColor3f(0, 1, 1);
glBegin(GL_LINES);
glVertex3f(originX, originY, -originZ);
glVertex3f(originX + zx * 100, originY + zy * 100, -(originZ + zz * 100));
glEnd();
glColor3f(1, 0, 1);
glBegin(GL_LINES);
glVertex3f(originX, originY, -originZ);
glVertex3f(originX + yx * 100, originY + yy * 100, -(originZ + yz * 100));
glEnd();
glColor3f(1, 1, 0);
glBegin(GL_LINES);
glVertex3f(originX, originY, -originZ);
glVertex3f(originX + xx * 100, originY + xy * 100, -(originZ + xz * 100));
glEnd();
Matrix<float> transform(4, 4);
Matrix<float> blue(4, 1);
Matrix<float> red(4, 1);
transform.put(0, 0, xx);
transform.put(1, 0, xy);
transform.put(2, 0, xz);
transform.put(3, 0, 0);
transform.put(0, 1, yx);
transform.put(1, 1, yy);
transform.put(2, 1, yz);
transform.put(3, 1, 0);
transform.put(0, 2, zx);
transform.put(1, 2, zy);
transform.put(2, 2, zz);
transform.put(3, 2, 0);
transform.put(0, 3, originX);
transform.put(1, 3, originY);
transform.put(2, 3, originZ);
transform.put(3, 3, 1);
blue.put(0, 0, blueX);
blue.put(1, 0, blueY);
blue.put(2, 0, blueZ);
blue.put(3, 0, 1);
red.put(0, 0, redX);
red.put(1, 0, redY);
red.put(2, 0, redZ);
red.put(3, 0, 1);
Matrix<float> localBlue = transform.getInverse() * blue;
Matrix<float> localRed = transform.getInverse() * red;
printf("Blue: %f, %f, %f\n", localBlue.get(0, 0), localBlue.get(1, 0), localBlue.get(2, 0));
printf("Red: %f, %f, %f\n", localRed.get(0, 0), localRed.get(1, 0), localRed.get(2, 0));
fflush(stdout);
// Swap the OpenGL display buffers
glutSwapBuffers();
first = true;
}
else
{
m_color.readFrame(&m_colorFrame);
m_colorData = (const openni::RGB888Pixel*)m_colorFrame.getData();
first = false;
}
}
const PPM_Color PPM_Image::color(const int x, const int y) const {
const uint c {vals_[x][y]};
return PPM_Color {red(c), green(c), blue(c)};
}
void
GUIParkingArea::drawGL(const GUIVisualizationSettings& s) const {
glPushName(getGlID());
glPushMatrix();
RGBColor grey(177, 184, 186, 171);
RGBColor blue(83, 89, 172, 255);
RGBColor red(255, 0, 0, 255);
RGBColor green(0, 255, 0, 255);
// draw the area
glTranslated(0, 0, getType());
GLHelper::setColor(blue);
GLHelper::drawBoxLines(myShape, myShapeRotations, myShapeLengths, myWidth / 2.);
// draw details unless zoomed out to far
const SUMOReal exaggeration = s.addSize.getExaggeration(s);
if (s.scale * exaggeration >= 10) {
// draw the lots
glTranslated(0, 0, .1);
std::map<unsigned int, LotSpaceDefinition >::const_iterator i;
for (i = mySpaceOccupancies.begin(); i != mySpaceOccupancies.end(); i++) {
glPushMatrix();
glTranslated((*i).second.myPosition.x(), (*i).second.myPosition.y(), (*i).second.myPosition.z());
glRotated((*i).second.myRotation, 0, 0, 1);
Position pos = (*i).second.myPosition;
PositionVector geom;
SUMOReal w = (*i).second.myWidth / 2.;
SUMOReal h = (*i).second.myLength;
geom.push_back(Position(- w, + 0, 0.));
geom.push_back(Position(+ w, + 0, 0.));
geom.push_back(Position(+ w, + h, 0.));
geom.push_back(Position(- w, + h, 0.));
geom.push_back(Position(- w, + 0, 0.));
/*
geom.push_back(Position(pos.x(), pos.y(), pos.z()));
geom.push_back(Position(pos.x() + (*l).second.myWidth, pos.y(), pos.z()));
geom.push_back(Position(pos.x() + (*l).second.myWidth, pos.y() - (*l).second.myLength, pos.z()));
geom.push_back(Position(pos.x(), pos.y() - (*l).second.myLength, pos.z()));
geom.push_back(Position(pos.x(), pos.y(), pos.z()));
*/
GLHelper::setColor((*i).second.vehicle == 0 ? green : red);
GLHelper::drawBoxLines(geom, 0.1);
glPopMatrix();
}
GLHelper::setColor(blue);
// draw the lines
for (size_t i = 0; i != myLines.size(); ++i) {
glPushMatrix();
glTranslated(mySignPos.x(), mySignPos.y(), 0);
glRotated(180, 1, 0, 0);
glRotated(mySignRot, 0, 0, 1);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
pfSetPosition(0, 0);
pfSetScale(1.f);
glScaled(exaggeration, exaggeration, 1);
glTranslated(1.2, -(double)i, 0);
pfDrawString(myLines[i].c_str());
glPopMatrix();
}
// draw the sign
glTranslated(mySignPos.x(), mySignPos.y(), 0);
int noPoints = 9;
if (s.scale * exaggeration > 25) {
noPoints = MIN2((int)(9.0 + (s.scale * exaggeration) / 10.0), 36);
}
glScaled(exaggeration, exaggeration, 1);
GLHelper::drawFilledCircle((SUMOReal) 1.1, noPoints);
glTranslated(0, 0, .1);
GLHelper::setColor(grey);
GLHelper::drawFilledCircle((SUMOReal) 0.9, noPoints);
if (s.scale * exaggeration >= 4.5) {
GLHelper::drawText("P", Position(), .1, 1.6 * exaggeration, blue, mySignRot);
}
}
glPopMatrix();
glPopName();
drawName(getCenteringBoundary().getCenter(), s.scale, s.addName);
for (std::vector<MSTransportable*>::const_iterator i = myWaitingTransportables.begin(); i != myWaitingTransportables.end(); ++i) {
glTranslated(0, 1, 0); // make multiple containers viewable
static_cast<GUIContainer*>(*i)->drawGL(s);
}
}
inline std::string to_string() const
{
std::stringstream ss;
ss << "rgb (" << red() << "," << green() << "," << blue() <<")";
return ss.str();
}
std::vector<Dimension> MrsidReader::getDefaultDimensions()
{
std::vector<Dimension> output;
Dimension x("X", dimension::Float, 8);
x.setUUID("8c54ff0c-234f-43a2-8959-d9681ad1dea3");
x.setNamespace(s_getName());
output.push_back(x);
Dimension y("Y", dimension::Float, 8);
y.setUUID("9cee971b-2505-40cd-b7e9-f32c399afac7");
y.setNamespace(s_getName());
output.push_back(y);
Dimension z("Z", dimension::Float, 8);
z.setUUID("89dc4e36-6166-4bc8-bf95-5660657b0ea6");
z.setNamespace(s_getName());
output.push_back(z);
Dimension t("Time", dimension::Float, 8);
t.setUUID("3aea2826-4a6e-4c1b-ae27-7b2f24763ce1");
t.setNamespace(s_getName());
output.push_back(t);
Dimension blue("Blue", dimension::UnsignedInteger, 2);
blue.setUUID("f69977e3-23e8-4483-91b6-14cad981e9fd");
blue.setNamespace(s_getName());
output.push_back(blue);
Dimension red("Red", dimension::UnsignedInteger, 2);
red.setUUID("40a02a80-c399-42f0-a587-a286635b446e");
red.setNamespace(s_getName());
output.push_back(red);
Dimension green("Green", dimension::UnsignedInteger, 2);
green.setUUID("b329df2d-44f1-4f35-8993-d183dacaa1ff");
green.setNamespace(s_getName());
output.push_back(green);
Dimension cls("Classification", dimension::UnsignedInteger, 1);
cls.setUUID("ba1e2af8-6dfd-46a7-972a-ecc00f68914d");
cls.setNamespace(s_getName());
output.push_back(cls);
Dimension edge("EdgeOfFlightLine", dimension::UnsignedInteger, 1);
edge.setUUID("daed3bfc-650d-4265-93a7-d8093cbd75a0");
edge.setNamespace(s_getName());
output.push_back(edge);
Dimension intensity("Intensity", dimension::UnsignedInteger, 2);
intensity.setUUID("a43332a8-26b3-4609-a2b1-ddfda30e0565");
intensity.setNamespace(s_getName());
output.push_back(intensity);
Dimension num_returns("NumberOfReturns", dimension::UnsignedInteger, 1);
num_returns.setUUID("fa7c5d56-fb2b-4f81-9126-f183000c3cd8");
num_returns.setNamespace(s_getName());
output.push_back(num_returns);
Dimension return_no("ReturnNumber", dimension::UnsignedInteger, 1);
return_no.setUUID("e38cc121-8d26-482a-8920-c5599b2cdd19");
return_no.setNamespace(s_getName());
output.push_back(return_no);
Dimension scan_angle("ScanAngleRank", dimension::UnsignedInteger, 1);
scan_angle.setUUID("5d816875-10a5-4048-ad9d-fd3b8d065a6a");
scan_angle.setNamespace(s_getName());
output.push_back(scan_angle);
Dimension scan_dir("ScanDirectionFlag", dimension::UnsignedInteger, 1);
scan_dir.setUUID("d1054379-8bf6-4685-abfc-1f0ec37aa819");
scan_dir.setNamespace(s_getName());
output.push_back(scan_dir);
Dimension ptsource("PointSourceId", dimension::UnsignedInteger, 2);
ptsource.setUUID("be6e71af-b2f7-4107-a902-96e2fb71343f");
ptsource.setNamespace(s_getName());
output.push_back(ptsource);
Dimension userdata("UserData", dimension::UnsignedInteger, 1);
userdata.setUUID("551ca4be-cb6e-47a4-93a9-e403e9a06a8a");
userdata.setNamespace(s_getName());
output.push_back(userdata);
return output;
}
void MapFrame::paintEvent(QPaintEvent* event)
{
// Colorblind friendly colors
QColor green(17, 192, 131);
QColor red(255, 65, 30);
float max_seen_height = -std::numeric_limits<float>::max(); // std::numeric_limits<float>::max() is the max possible floating point value
float max_seen_width = -std::numeric_limits<float>::max();
float min_seen_x = std::numeric_limits<float>::max();
float min_seen_y = std::numeric_limits<float>::max();
// Set the max and min seen values depending on which data the user has selected to view
// Check each of the display data options and choose the most extreme value from those selected by the user
if (display_ekf_data)
{
min_seen_x = min_ekf_seen_x[rover_to_display];
min_seen_y = min_ekf_seen_y[rover_to_display];
max_seen_height = max_ekf_seen_height[rover_to_display];
max_seen_width = max_ekf_seen_height[rover_to_display];
}
if (display_gps_data)
{
if (min_seen_x > min_gps_seen_x[rover_to_display]) min_seen_x = min_gps_seen_x[rover_to_display];
if (min_seen_y > min_gps_seen_y[rover_to_display]) min_seen_y = min_gps_seen_y[rover_to_display];
if (max_seen_height < max_gps_seen_height[rover_to_display]) max_seen_height = max_gps_seen_height[rover_to_display];
if (max_seen_width < max_gps_seen_width[rover_to_display]) max_seen_width = max_gps_seen_width[rover_to_display];
}
if (display_encoder_data)
{
if (min_seen_x > min_encoder_seen_x[rover_to_display]) min_seen_x = min_encoder_seen_x[rover_to_display];
if (min_seen_y > min_encoder_seen_y[rover_to_display]) min_seen_y = min_encoder_seen_y[rover_to_display];
if (max_seen_height < max_encoder_seen_height[rover_to_display]) max_seen_height = max_encoder_seen_height[rover_to_display];
if (max_seen_width < max_encoder_seen_width[rover_to_display]) max_seen_width = max_encoder_seen_width[rover_to_display];
}
// Begin drawing the map
QPainter painter(this);
painter.setPen(Qt::white);
// Track the frames per second for development purposes
QString frames_per_second;
frames_per_second = QString::number(frames /(frame_rate_timer.elapsed() / 1000.0), 'f', 0) + " FPS";
QFontMetrics fm(painter.font());
painter.drawText(this->width()-fm.width(frames_per_second), fm.height(), frames_per_second);
frames++;
if (!(frames % 100)) // time how long it takes to dispay 100 frames
{
frame_rate_timer.start();
frames = 0;
}
// end frames per second
if (ekf_rover_path[rover_to_display].empty() && encoder_rover_path[rover_to_display].empty() && gps_rover_path[rover_to_display].empty() && target_locations[rover_to_display].empty() && collection_points[rover_to_display].empty())
{
painter.drawText(QPoint(50,50), "Map Frame: Nothing to display.");
return;
}
int map_origin_x = fm.width(QString::number(-max_seen_height, 'f', 1)+"m");
int map_origin_y = 2*fm.height();
int map_width = this->width()-map_origin_x;
int map_height = this->height()-map_origin_y;
int map_center_x = map_origin_x+((map_width-map_origin_x)/2);
int map_center_y = map_origin_y+((map_height-map_origin_y)/2);
// Draw the scale bars
//painter.setPen(Qt::gray);
//painter.drawLine(QPoint(map_center_x, map_origin_y), QPoint(map_center_x, map_height));
//painter.drawLine(QPoint(map_origin_x, map_center_y), QPoint(map_width, map_center_y));
//painter.setPen(Qt::white);
// Cross hairs at map display center
QPoint axes_origin(map_origin_x,map_origin_y);
QPoint x_axis(map_width,map_origin_y);
QPoint y_axis(map_origin_x,map_height);
painter.drawLine(axes_origin, x_axis);
painter.drawLine(axes_origin, y_axis);
painter.drawLine(QPoint(map_width, map_origin_y), QPoint(map_width, map_height));
painter.drawLine(QPoint(map_origin_x, map_height), QPoint(map_width, map_height));
// Draw north arrow
QPoint northArrow_point(map_center_x, 0);
QPoint northArrow_left(map_center_x - 5, 5);
QPoint northArrow_right(map_center_x + 5, 5);
QRect northArrow_textBox(northArrow_left.x(), northArrow_left.y(), 10, 15);
painter.drawLine(northArrow_left, northArrow_right);
painter.drawLine(northArrow_left, northArrow_point);
painter.drawLine(northArrow_right, northArrow_point);
painter.drawText(northArrow_textBox, QString("N"));
// Draw rover origin crosshairs
// painter.setPen(green);
// Check encoder has any values in it
if (ekf_rover_path[rover_to_display].empty())
{
painter.drawText(QPoint(50,50), "Map Frame: No encoder data received.");
return;
}
float initial_x = 0.0; //ekf_rover_path[rover_to_display].begin()->first;
float initial_y = 0.001; //ekf_rover_path[rover_to_display].begin()->second;
float rover_origin_x = map_origin_x+((initial_x-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
float rover_origin_y = map_origin_y+((initial_y-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
painter.setPen(Qt::gray);
painter.drawLine(QPoint(rover_origin_x, map_origin_y), QPoint(rover_origin_x, map_height));
painter.drawLine(QPoint(map_origin_x, rover_origin_y), QPoint(map_width, rover_origin_y));
painter.setPen(Qt::white);
int n_ticks = 6;
float tick_length = 5;
QPoint x_axis_ticks[n_ticks];
QPoint y_axis_ticks[n_ticks];
for (int i = 0; i < n_ticks-1; i++)
{
x_axis_ticks[i].setX(axes_origin.x()+(i+1)*map_width/n_ticks);
x_axis_ticks[i].setY(axes_origin.y());
y_axis_ticks[i].setX(axes_origin.x());
y_axis_ticks[i].setY(axes_origin.y()+(i+1)*map_height/n_ticks);
}
for (int i = 0; i < n_ticks-1; i++)
{
painter.drawLine(x_axis_ticks[i], QPoint(x_axis_ticks[i].x(), x_axis_ticks[i].y()+tick_length));
painter.drawLine(y_axis_ticks[i], QPoint(y_axis_ticks[i].x()+tick_length, y_axis_ticks[i].y()));
}
for (int i = 0; i < n_ticks-1; i++)
{
float fraction_of_map_to_rover_x = (rover_origin_x-map_origin_x)/map_width;
float fraction_of_map_to_rover_y = (rover_origin_y-map_origin_y)/map_height;
float x_label_f = (i+1)*max_seen_width/n_ticks-fraction_of_map_to_rover_x*max_seen_width;
float y_label_f = (i+1)*max_seen_height/n_ticks-fraction_of_map_to_rover_y*max_seen_height;
QString x_label = QString::number(x_label_f, 'f', 1) + "m";
QString y_label = QString::number(-y_label_f, 'f', 1) + "m";
int x_labels_offset_x = -(fm.width(x_label))/2;
int x_labels_offset_y = 0;
int y_labels_offset_x = -(fm.width(y_label));
int y_labels_offset_y = fm.height()/3;
painter.drawText(x_axis_ticks[i].x()+x_labels_offset_x, axes_origin.y()+x_labels_offset_y, x_label);
painter.drawText(axes_origin.x()+y_labels_offset_x, y_axis_ticks[i].y()+y_labels_offset_y, y_label);
}
// End draw scale bars
update_mutex.lock();
// scale coordinates
std::vector<QPoint> scaled_target_locations;
for(std::vector< pair<float,float> >::iterator it = target_locations[rover_to_display].begin(); it != target_locations[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
QPoint point;
point.setX(map_origin_x+coordinate.first*map_width);
point.setY(map_origin_y+coordinate.second*map_height);
scaled_target_locations.push_back(point);
}
std::vector<QPoint> scaled_collection_points;
for(std::vector< pair<float,float> >::iterator it = collection_points[rover_to_display].begin(); it != collection_points[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
QPoint point;
point.setX(map_origin_x+coordinate.first*map_width);
point.setY(map_origin_y+coordinate.second*map_height);
scaled_collection_points.push_back(point);
}
std::vector<QPoint> scaled_gps_rover_points;
for(std::vector< pair<float,float> >::iterator it = gps_rover_path[rover_to_display].begin(); it != gps_rover_path[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
scaled_gps_rover_points.push_back( QPoint(x,y) );
}
QPainterPath scaled_ekf_rover_path;
for(std::vector< pair<float,float> >::iterator it = ekf_rover_path[rover_to_display].begin(); it != ekf_rover_path[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
QPoint point;
float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
if (it == ekf_rover_path[rover_to_display].begin()) scaled_ekf_rover_path.moveTo(x, y);
scaled_ekf_rover_path.lineTo(x, y);
}
QPainterPath scaled_gps_rover_path;
for(std::vector< pair<float,float> >::iterator it = gps_rover_path[rover_to_display].begin(); it != gps_rover_path[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
QPoint point;
float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
scaled_gps_rover_path.lineTo(x, y);
}
QPainterPath scaled_encoder_rover_path;
for(std::vector< pair<float,float> >::iterator it = encoder_rover_path[rover_to_display].begin(); it != encoder_rover_path[rover_to_display].end(); ++it) {
pair<float,float> coordinate = *it;
QPoint point;
float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
if (it == encoder_rover_path[rover_to_display].begin()) scaled_encoder_rover_path.moveTo(x, y);
scaled_encoder_rover_path.lineTo(x, y);
}
painter.setPen(red);
if (display_gps_data) painter.drawPoints(&scaled_gps_rover_points[0], scaled_gps_rover_points.size());
// if (display_gps_data) painter.drawPath(scaled_gps_rover_path);
painter.setPen(Qt::white);
if (display_ekf_data) painter.drawPath(scaled_ekf_rover_path);
painter.setPen(green);
if (display_encoder_data) painter.drawPath(scaled_encoder_rover_path);
painter.setPen(red);
QPoint* point_array = &scaled_collection_points[0];
painter.drawPoints(point_array, scaled_collection_points.size());
painter.setPen(green);
point_array = &scaled_target_locations[0];
painter.drawPoints(point_array, scaled_target_locations.size());
update_mutex.unlock();
painter.setPen(Qt::white);
}
void DrawWorld(wxMemoryDC &mem,struct Map *floorplancopy)
{
wxBrush blackback(wxColour(0,0,0),wxSOLID);
wxPen white(wxColour(255,255,255),1,wxSOLID);
wxBrush whiteback(wxColour(255,255,255),wxSOLID);
wxBrush grayback(wxColour(123,123,123),wxSOLID);
wxBrush redback(wxColour(255,0,0),wxSOLID);
mem.SetPen(white);
mem.SetBrush(whiteback);
mem.DrawRectangle(0,0,draw_area_width,draw_area_height);
wxPen black(wxColour(0,0,0),1,wxSOLID);
wxPen red(wxColour(255,0,0),1,wxSOLID);
int obj=0;
mem.SetPen(black);
mem.SetBrush(whiteback);
for (unsigned int y =0; y<draw_area_actual_pointsY; y++ )
{
for (unsigned int x =0; x<draw_area_actual_pointsX; x++ )
{
obj = ObstacleExists(GetWorldHandler(),x,y);
if ( obj!=0 )
{
mem.SetPen(red);
mem.SetBrush(blackback);
} else
{
obj = ObstacleRadiousExists(GetWorldHandler(),x,y);
if ( obj!=0 )
{
mem.SetPen(black);
mem.SetBrush(grayback);
}
}
mem.DrawRectangle(x*map_box_size,y*map_box_size,map_box_size,map_box_size);
obj=1;
if ( obj!=0 )
{
mem.SetPen(white); //black for boxes everywhere!
mem.SetBrush(whiteback);
obj = 0;
}
}
}
DrawSolvePath(mem);
unsigned int endx=0,endy=0;
GetAgentTargetLocation(GetWorldHandler(),OURROBOT,&endx,&endy);
DrawEndPoint(mem,endx,endy);
// Draw Start point drawn second in case both points are the same ( we are on target :P )
unsigned int startx=0,starty=0;
GetAgentLocation(GetWorldHandler(),OURROBOT,&startx,&starty);
DrawStartPoint(mem,startx,starty,GetAgentHeading(GetWorldHandler(),OURROBOT));
}
int SolidifyPW::Display(
TimeValue t, INode* inode, ViewExp *vpt, int flags,
ModContext *mc)
{
return 0;
GraphicsWindow *gw = vpt->getGW();
// Transform the gizmo with the node.
Matrix3 ntm = inode->GetObjectTM(t);
gw->setTransform(ntm);
Point3 selGizmoColor = GetUIColor(COLOR_SEL_GIZMOS);
Point3 gizmoColor = GetUIColor(COLOR_GIZMOS);
Point3 green(0.0f,1.0f,0.0f);
Point3 blue(0.0f,0.0f,1.0f);
Point3 red(1.0f,0.0f,0.0f);
gw->setColor(LINE_COLOR, gizmoColor);
for (int i = 0; i < meshInfo.bevelData.Count(); i++)
{
if (meshInfo.bevelData[i].ct == 2)
{
Point3 l[3];
gw->setColor(LINE_COLOR, gizmoColor);
l[0] = meshInfo.bevelData[i].p;
l[1] = meshInfo.bevelData[i].p + meshInfo.bevelData[i].vec[0]*1.f;
gw->polyline(2, l, NULL, NULL, 0);
l[1] = meshInfo.bevelData[i].p + meshInfo.bevelData[i].vec[1]*1.f;
gw->polyline(2, l, NULL, NULL, 0);
gw->setColor(LINE_COLOR, selGizmoColor);
l[1] = meshInfo.bevelData[i].p + meshInfo.bevelData[i].vnorm*10.f;
gw->polyline(2, l, NULL, NULL, 0);
gw->setColor(LINE_COLOR, red);
l[1] = meshInfo.bevelData[i].p + meshInfo.bevelData[i].norm*1.f*meshInfo.bevelData[i].realLength;
gw->polyline(2, l, NULL, NULL, 0);
/*
gw->setColor(LINE_COLOR, green);
l[1] = bevelData[i].p + bevelData[i].cross[0]*5.f;
gw->polyline(2, l, NULL, NULL, 0);
gw->setColor(LINE_COLOR, blue );
l[1] = bevelData[i].p + bevelData[i].cross[1]*5.f;
gw->polyline(2, l, NULL, NULL, 0);
*/
}
}
return 0;
/*
for (int i = 0; i < vPlanes.Count(); i++)
{
if (vPlanes[i].display)
{
gw->setColor(LINE_COLOR, gizmoColor);
gw->marker(&vPlanes[i].debugP1,PLUS_SIGN_MRKR);
gw->marker(&vPlanes[i].debugP2,PLUS_SIGN_MRKR);
Point3 l[3];
l[0] = vPlanes[i].debugP1;
l[1] = vPlanes[i].debugP2;
gw->polyline(2, l, NULL, NULL, 0);
gw->setColor(LINE_COLOR, selGizmoColor);
gw->marker(&vPlanes[i].debugBase,PLUS_SIGN_MRKR);
l[0] = vPlanes[i].debugBase;
l[1] = vPlanes[i].debugBase+(vPlanes[i].debugNorm);
gw->polyline(2, l, NULL, NULL, 0);
gw->setColor(LINE_COLOR, red);
l[0] = vPlanes[i].debugBase;
l[1] = vPlanes[i].debugBase+(vPlanes[i].vnorm);
gw->polyline(2, l, NULL, NULL, 0);
}
}
*/
for (int i = 0; i < meshInfo.op.Count(); i++)
{
gw->setColor(LINE_COLOR, selGizmoColor);
gw->marker(&meshInfo.cdata[i].p,PLUS_SIGN_MRKR);
Point3 l[3];
l[0] = meshInfo.op[i];
l[1] = meshInfo.op[i] + meshInfo.vnorms[i];
gw->polyline(2, l, NULL, NULL, 0);
gw->setColor(LINE_COLOR, gizmoColor);
// l[0] = cdata[i].p;
// l[1] = cdata[i].p + cdata[i].hyp;
//
// gw->polyline(2, l, NULL, NULL, 0);
// l[0] = cdata[i].p + cdata[i].norm;;
// l[1] = cdata[i].p + cdata[i].hyp;
// gw->setColor(LINE_COLOR, red);
// gw->polyline(2, l, NULL, NULL, 0);
}
return 0;
}
String Color::nameForRenderTreeAsText() const
{
if (alpha() < 0xFF)
return String::format("#%02X%02X%02X%02X", red(), green(), blue(), alpha());
return String::format("#%02X%02X%02X", red(), green(), blue());
}
void LED_toggling(){
green();
red();
}
// --------------
// -----Main-----
// --------------
int
main (int argc, char** argv)
{
// --------------------------------------
// -----Parse Command Line Arguments-----
// --------------------------------------
double normals_radius= 0.005;
if (pcl::console::parse (argc, argv, "-n", normals_radius) >= 0)
{
std::cout << " Radius: " << normals_radius << "\n";
}
double harris_radius = 0.5;
if (pcl::console::parse (argc, argv, "--hariss_radius", harris_radius) >= 0)
{
std::cout << " harris_radius: " << harris_radius << "\n";
}
double harris_search_radius = 0.01;
if (pcl::console::parse (argc, argv, "--harris_search_radius", harris_search_radius) >= 0)
{
std::cout << " harris_search_radius: " << harris_search_radius << "\n";
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (argv[1], *cloudRGB) == -1) //* load the file
{
std::cout << "Couldn't read file cloud.pcd ";
return (-1);
}
pcl::copyPointCloud(*cloudRGB, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr harris3d (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr iss3d (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr susan (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr harris6d (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sift (new pcl::PointCloud<pcl::PointXYZ>);
NormalCloudPtr normals(new NormalCloud());
pcl::NormalEstimationOMP<PointT, NormalT> est;
est.setRadiusSearch(normals_radius);
est.setInputCloud(cloud);
est.compute(*normals);
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints_temp(
new pcl::PointCloud<pcl::PointXYZI>());
HarrisKeypoint* detector = new HarrisKeypoint(HarrisKeypoint::HARRIS);
detector->setNonMaxSupression(true);
detector->setRadius(harris_radius);
detector->setRadiusSearch(harris_search_radius);
detector->setMethod(HarrisKeypoint::HARRIS);
// detector->setKSearch();
detector->setNumberOfThreads(10);
detector->setSearchMethod(tree);
// detector->setThreshold();
// detector->use_indices_ = false;
detector->setInputCloud(cloud);
detector->compute(*keypoints_temp);
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints(
new pcl::PointCloud<pcl::PointXYZ>());
pcl::copyPointCloud(*keypoints_temp, *harris3d);
}
std::cout << " harris3d size :" << harris3d->size() << std::endl;;
/*
{
double iss_salient_radius_;
double iss_non_max_radius_;
double iss_gamma_21_ (0.975);
double iss_gamma_32_ (0.975);
double iss_min_neighbors_ (20);
int iss_threads_ (10);
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
// Fill in the model cloud
double model_resolution = 0.01;
// Compute model_resolution
iss_salient_radius_ = 6 * model_resolution;
iss_non_max_radius_ = 4 * model_resolution;
//
// Compute keypoints
//
pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_detector;
iss_detector.setSearchMethod (tree);
iss_detector.setSalientRadius (iss_salient_radius_);
iss_detector.setNonMaxRadius (iss_non_max_radius_);
iss_detector.setThreshold21 (iss_gamma_21_);
iss_detector.setThreshold32 (iss_gamma_32_);
iss_detector.setMinNeighbors (iss_min_neighbors_);
iss_detector.setNumberOfThreads (iss_threads_);
iss_detector.setInputCloud (cloud);
iss_detector.compute (*iss3d);
}
std::cout << " iss3d size :" << iss3d->size() << std::endl;
{
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
pcl::SUSANKeypoint<pcl::PointXYZRGB, pcl::PointXYZRGB>* susan3D = new pcl::SUSANKeypoint<pcl::PointXYZRGB, pcl::PointXYZRGB>;
susan3D->setInputCloud(cloudRGB);
susan3D->setNonMaxSupression(true);
susan3D->setSearchMethod(tree);
susan3D->setRadius(harris_radius);
susan3D->setRadiusSearch(harris_search_radius);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr keypoints (new pcl::PointCloud<pcl::PointXYZRGB> ());
susan3D->compute(*keypoints);
pcl::PointCloud<pcl::PointXYZ>::Ptr susan(new pcl::PointCloud<pcl::PointXYZ>());
pcl::copyPointCloud(*keypoints, *susan);
}
std::cout << " susan size :" << susan->size() << std::endl;
{
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints_temp(
new pcl::PointCloud<pcl::PointXYZI>());
HarrisKeypoint6D* detector = new HarrisKeypoint6D(HarrisKeypoint::HARRIS);
detector->setNonMaxSupression(true);
detector->setRadius(harris_radius);
detector->setRadiusSearch(harris_search_radius);
// detector->setMethod(HarrisKeypoint::HARRIS);
detector->setNumberOfThreads(10);
detector->setSearchMethod(tree);
// detector->setThreshold();
// detector->use_indices_ = false;
detector->setInputCloud(cloudRGB);
detector->compute(*keypoints_temp);
pcl::copyPointCloud(*keypoints_temp, *harris6d);
}
std::cout << " harris6d size :" << harris6d->size() << std::endl;
/*
*/
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> white (cloud, 255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red (harris3d, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> green (iss3d, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue (susan, 0, 0, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> col2 (harris6d, 255, 0, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> col3 (sift, 255, 255, 0);
//////
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ> (cloud, white, "cloud");
viewer->addPointCloud<pcl::PointXYZ> (harris3d, red, "keypoints2");
viewer->addPointCloud<pcl::PointXYZ> (iss3d, green, "keypoints3");
viewer->addPointCloud<pcl::PointXYZ> (susan, blue, "keypoint4s");
viewer->addPointCloud<pcl::PointXYZ> (harris6d, col2, "keypoin3ts");
viewer->addPointCloud<pcl::PointXYZ> (sift, col3, "keypoint36");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoints2");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoints3");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoint4s");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoin3ts");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "keypoint36");
// viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud, normals, 5, 0.05, "normals");
viewer->addCoordinateSystem (1.0, "global");
viewer->initCameraParameters ();
// viewer->addSphere (cloud->points[0], normals_radius, "sphere+norm", 0);
// viewer->addSphere (cloud->points[0], shot_radius, "sphere+shot", 0);
//--------------------
// -----Main loop-----
//--------------------
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
void GameManager::init() {
glShadeModel(GL_SMOOTH);
_hud = new Hud(lives);
/*
LIGHT SOURCES INITIALIZATIONS
*/
//AMBIENT
Vector3 ambient_direction (0, 0, -1);
Vector3 blue(0, 0, 1.0);
Vector3 red(1.0, 0, 0);
Vector3 green(0, 1.0, 0);
Vector3 candle_light(1.0, 0.58, 0.16);
_light_sources.push_back(new LightSource(GL_LIGHT0));
_light_sources[0]->setDirection(ambient_direction);
_light_sources[0]->setAmbient(ambient_day);
_light_sources[0]->setDiffuse(ambient_day);
_light_sources[0]->setSpecular(ambient_day);
_light_sources[0]->setPosition(1280/2, 720/2, 1000, 0.0);
_light_sources[0]->draw();
//CANDLES
_light_sources.push_back(new LightSource(GL_LIGHT1));
_light_sources.push_back(new LightSource(GL_LIGHT2));
_light_sources.push_back(new LightSource(GL_LIGHT3));
_light_sources.push_back(new LightSource(GL_LIGHT4));
_light_sources.push_back(new LightSource(GL_LIGHT5));
_light_sources.push_back(new LightSource(GL_LIGHT6));
_light_sources.push_back(new LightSource(GL_LIGHT7));
/*
CAMERA
*/
_active_camera = new OrthogonalCamera(0, 1280, 0, 720, -1000, 1000);
_cameras.push_back(_active_camera);
_cameras.push_back(new PerspectiveCamera(70, 16 / 9, 100, 2000, 1280 / 2 , 720 / 2, 0, 0, 0, 1));
_cameras[1]->setPosition(1280/2, -200,1000);
_cars.push_back(new Car(_light_sources[6], _light_sources[7]));
_cars[0]->setLives(lives);
_cars[0]->headlights_toggle(false);
_road = new Roadside(track1, 209, -60, -200, 0);
_game_objects.push_back(_cars[0]);
_game_objects.push_back(_road);
_game_objects.push_back(new Orange(800, 360, 0, 30));
_game_objects.push_back(new Orange(130, 640, 0, 30));
_game_objects.push_back(new Orange(1050, 100, 0, 55));
_game_objects.push_back(new Butter());
_game_objects.push_back(new Butter());
_game_objects.push_back(new Butter());
_game_objects.push_back(new Butter());
_game_objects.push_back(new Butter());
//non dynamic
_game_objects[0]->setPosition(105, 300, 0);
_game_objects[2]->setSpeed(-1, 1, 0);
_game_objects[3]->setSpeed(1, -1, 0);
_game_objects[4]->setSpeed(1, 1, 0);
/*
Dynamic Objects
*/
//butter
_game_objects[5]->setPosition(400, 220, 0);
_game_objects[5]->setRotation(45);
_game_objects[6]->setPosition(200, 450, 0);
_game_objects[6]->setRotation(40);
_game_objects[7]->setPosition(850, 425, 0);
_game_objects[7]->setRotation(130);
_game_objects[8]->setPosition(1220, 400, 0);
_game_objects[8]->setRotation(90);
_game_objects[9]->setPosition(650, 550, 0);
_cameras.push_back(new PerspectiveCamera(75, 1280/720, 10, 2000,
_cars[0]->getPosition()->getX(), _cars[0]->getPosition()->getY(), 0, 0, 0, 1));
_game_objects.push_back(new Candle(250, 200, 0, _light_sources[1], candle_light));
_game_objects.push_back(new Candle(200, 650, 0, _light_sources[2], candle_light));
_game_objects.push_back(new Candle(1000, 300, 0, _light_sources[3], candle_light));
_game_objects.push_back(new Candle(1000, 500, 0, _light_sources[4], candle_light));
_game_objects.push_back(new Candle(330, 430, 0, _light_sources[5], candle_light));
_cameras.push_back(new OrthogonalCamera(0, 1280, 0, 720, -1000, 1000));
}
static Uint32 max_rgb(Uint32 first,Uint32 second)
{ return rgb(std::max(red(first),red(second)),std::max(green(first),green(second)),std::max(blue(first),blue(second))) ; }
Canvas::RGB
Canvas::RGB::scale(const double a)
{
return RGB(a * red(), a * green(), a * blue());
}
void OptionsDialog2::Draw(DrawTexture *drawTexture, float alpha) {
float x, y;
this->GetMousePosition(x, y);
glm::vec3 black(-1,-1,-1), white(0,0,0), red(0, -1, -1);
// Draw the dialog background.
dialog::Draw(drawTexture, alpha);
fMenuFont->Enable();
fMenuFont->UpdateProjection();
glm::vec3 color;
fActiveElement = ActiveElement::NONE;
fTitle.Draw(black, alpha);
color = black;
if (fPing.Inside(x, y) || fPingMark.Inside(x, y)) {
color = white;
fActiveElement = ActiveElement::PING;
}
fPing.Draw(color, alpha);
if (gShowPing)
fPingMark.Draw(color, alpha);
color = black;
if (fMusic.Inside(x, y) || fMusicMark.Inside(x, y)) {
color = white;
fActiveElement = ActiveElement::MUSIC;
}
fMusic.Draw(color, alpha);
if (Options::fgOptions.fMusicOn)
fMusicMark.Draw(color, alpha);
fPerformance.Draw(black, alpha);
color = black;
if (fFullScreen.Inside(x, y) || fFullScreenMark.Inside(x, y)) {
color = white;
fActiveElement = ActiveElement::FULLSCREEN;
}
fFullScreen.Draw(color, alpha);
if (fNewFullScreen)
fFullScreenMark.Draw(color, alpha);
fPerformance.Draw(black, alpha);
glm::vec3 c1 = black, c2 = black, c3 = black, c4 = black;
switch (fNewPerfLevel) {
case 1: c1 = red; break;
case 2: c2 = red; break;
case 3: c3 = red; break;
case 4: c4 = red; break;
}
if (fPerf1.Inside(x,y)) { c1 = white; fActiveElement = ActiveElement::P1; }
if (fPerf2.Inside(x,y)) { c2 = white; fActiveElement = ActiveElement::P2; }
if (fPerf3.Inside(x,y)) { c3 = white; fActiveElement = ActiveElement::P3; }
if (fPerf4.Inside(x,y)) { c4 = white; fActiveElement = ActiveElement::P4; }
fPerf1.Draw(c1, alpha);
fPerf2.Draw(c2, alpha);
fPerf3.Draw(c3, alpha);
fPerf4.Draw(c4, alpha);
DialogText *close = &fPrevious;
if (this->RestartRequired())
close = &fCancel;
color = black;
if (close->Inside(x, y)) {
color = white;
fActiveElement = ActiveElement::PREVIOUS;
}
close->Draw(color, alpha);
if (this->RestartRequired()) {
color = black;
if (fQuit.Inside(x, y)) {
color = white;
fActiveElement = ActiveElement::SAVEANDQUIT;
}
fQuit.Draw(color, alpha);
} else {
color = black;
if (fAdvanced.Inside(x, y)) {
color = white;
fActiveElement = ActiveElement::ADVANCED;
}
fAdvanced.Draw(color, alpha);
}
}
Driver::Driver() :
r(red()), d(NULL), sigId(0) {}
int main (int argc, char** argv) {
const bool USE_OPENCV_DISPLAY = false; // Use OpenCV or CImg for display?
pI::BaseApplication app;
pI::Runtime runtime (app.GetRuntime());
LoadPlugins (app);
// Creates and initializes all the necessary plugins.
const int DeviceIndex = 0;
const bool ShowRGB = true;
// Freenect/GetVideoAndDepth returns both the RGB image and the depth map captured by a Kinect.
boost::shared_ptr<pI::pIn> getImageAndDepth
= app.SpawnAndInitialize (runtime,
"Freenect/GetVideoAndDepth",
DeviceIndex,
ShowRGB ? 0 : 2, // Video format: FREENECT_VIDEO_RGB or FREENECT_VIDEO_IR_8BIT
0 // Depth format: FREENECT_DEPTH_11BIT
);
pI::Arguments getImageAndDepth_in = getImageAndDepth->GetInputSignature(),
getImageAndDepth_out = getImageAndDepth->GetOutputSignature();
// Freenect/SetTiltAndLED sets the tilt angle and LED status of a Kinect.
boost::shared_ptr<pI::pIn> setTiltAndLED = app.SpawnAndInitialize (runtime, "Freenect/SetTiltAndLED", DeviceIndex);
pI::Arguments setTiltAndLED_in = setTiltAndLED->GetInputSignature(),
setTiltAndLED_out = setTiltAndLED->GetOutputSignature();
// Color/ApplyRGBPalette applies an arbitrary RGB palette onto a matrix
boost::shared_ptr<pI::pIn> applyRGB (runtime.SpawnPlugin ("pIn/Color/ApplyRGBPalette"));
pI::Arguments applyRGB_params = applyRGB->GetParameterSignature();
pI::RGBPalette rgb (applyRGB_params[0]);
rgb.SetCount (2048);
std::map<size_t, uint32_t> freenect_glview_colors; // Coloring as used in Freenect's glview example
freenect_glview_colors[0] = (0xFFFFFF); // white
freenect_glview_colors[256] = (0xFF0000); // red
freenect_glview_colors[512] = (0xFFFF00); // yellow
freenect_glview_colors[768] = (0x00FF00); // green
freenect_glview_colors[1024] = (0x00FFFF); // cyan
freenect_glview_colors[1280] = (0x0000FF); // blue
freenect_glview_colors[1536] = (0x000000); // black
std::map<size_t, uint32_t> terrain; // Alternative coloring; names taken from http://en.wikipedia.org/wiki/List_of_colors
terrain[0] = (0xFFFFFF); // White
terrain[256] = (0x964B00); // Brown (traditional)
terrain[512] = (0xFBEC5D); // Maize
terrain[768] = (0x66FF00); // Bright green
terrain[1024] = (0x014421); // Forest green (traditional)
terrain[1025] = (0x87CEFA); // Light sky blue
terrain[1280] = (0x00008B); // Dark blue
terrain[2047] = (0x000000); // Black
terrain[2048] = (0x696969); // Dim gray
ColorRamp ramp (terrain);
for (int i = 0; i < 2048; i++) {
float v = powf ( (i / 2048.), 3);
uint16_t gamma = 9216 * v;
uint32_t c = ramp.GetColor (gamma);
rgb.SetR (i, red (c));
rgb.SetG (i, green (c));
rgb.SetB (i, blue (c));
}
applyRGB->Initialize (applyRGB_params);
pI::Arguments applyRGB_in = applyRGB->GetInputSignature(),
applyRGB_out = applyRGB->GetOutputSignature();
// OpenCV/Draw/Rectangle draws the inner rectangle.
boost::shared_ptr<pI::pIn> drawRectImage = app.SpawnAndInitialize (runtime, "OpenCV/Draw/Rectangle");
pI::Arguments drawRectImage_in = drawRectImage->GetInputSignature(),
drawRectImage_out = drawRectImage->GetOutputSignature();
boost::shared_ptr<pI::pIn> drawRectDepth = app.SpawnAndInitialize (runtime, "OpenCV/Draw/Rectangle");
pI::Arguments drawRectDepth_in = drawRectDepth->GetInputSignature(),
drawRectDepth_out = drawRectDepth->GetOutputSignature();
// OpenCV/Draw/PutText draws a text string into an image.
boost::shared_ptr<pI::pIn> puttext
= app.SpawnAndInitialize (runtime,
"OpenCV/Draw/PutText",
0, // font type: CV_FONT_HERSHEY_SIMPLEX
pI_FALSE, // render font italic: no
0.75, 0.75, // horizontal and vertical scale
0.0, // shear: no shear
1, // thickness of the text strokes: 1
2 // type of the line: CV_AA
);
pI::Arguments puttext_in = puttext->GetInputSignature(),
puttext_out = puttext->GetOutputSignature();
// OpenCV/IO/SaveImage saves images to disk.
boost::shared_ptr<pI::pIn> saveImage (runtime.SpawnPlugin ("OpenCV/IO/SaveImage"));
saveImage->Initialize (saveImage->GetParameterSignature());
pI::Arguments saveImage_in = saveImage->GetInputSignature(),
saveImage_out = saveImage->GetOutputSignature();
// OpenCV/IO/ShowImage or CImg/Display displays the image and the depth map.
boost::shared_ptr<pI::pIn> displayImage, displayDepth;
if (USE_OPENCV_DISPLAY) {
displayImage = app.SpawnAndInitialize (runtime, "OpenCV/IO/ShowImage",
"RGB image window", false, 640, 480, 0, 0);
displayDepth = app.SpawnAndInitialize (runtime, "OpenCV/IO/ShowImage",
"Depth map window", false, 640, 480, 650, 0);
} else {
displayImage = app.SpawnAndInitialize (runtime, "CImg/Display", pI_TRUE);
displayDepth = app.SpawnAndInitialize (runtime, "CImg/Display", pI_TRUE);
}
pI::Arguments displayImage_in = displayImage->GetInputSignature(),
displayImage_out = displayImage->GetOutputSignature();
pI::Arguments displayDepth_in = displayDepth->GetInputSignature(),
displayDepth_out = displayDepth->GetOutputSignature();
// Now that all plugins are initialized, connect them.
drawRectImage_in[0] = getImageAndDepth_out[0];
pI::IntValue (drawRectImage_in[1]).SetData (310);
pI::IntValue (drawRectImage_in[2]).SetData (230);
pI::IntValue (drawRectImage_in[3]).SetData (330);
pI::IntValue (drawRectImage_in[4]).SetData (250);
pI::IntValue (drawRectImage_in[5]).SetData (255);
pI::IntValue (drawRectImage_in[6]).SetData (255);
pI::IntValue (drawRectImage_in[7]).SetData (255);
pI::IntValue (drawRectImage_in[8]).SetData (1);
pI::StringSelection (drawRectImage_in[9]).SetIndex (0);
pI::IntValue (drawRectImage_in[10]).SetData (0);
displayImage_in[0] = drawRectImage_in[0];
pI::StringValue fileName (saveImage_in[0]);
saveImage_in[1] = getImageAndDepth_out[0];
applyRGB_in[0] = getImageAndDepth_out[1];
drawRectDepth_in[0] = applyRGB_out[0];
pI::IntValue (drawRectDepth_in[1]).SetData (310);
pI::IntValue (drawRectDepth_in[2]).SetData (230);
pI::IntValue (drawRectDepth_in[3]).SetData (330);
pI::IntValue (drawRectDepth_in[4]).SetData (250);
pI::IntValue (drawRectDepth_in[5]).SetData (255);
pI::IntValue (drawRectDepth_in[6]).SetData (255);
pI::IntValue (drawRectDepth_in[7]).SetData (255);
pI::IntValue (drawRectDepth_in[8]).SetData (1);
pI::StringSelection (drawRectDepth_in[9]).SetIndex (0);
pI::IntValue (drawRectDepth_in[10]).SetData (0);
displayDepth_in[0] = drawRectDepth_in[0];
pI::StringValue puttext_text (puttext_in[1]);
pI::IntValue puttext_x (puttext_in[2]);
pI::IntValue puttext_y (puttext_in[3]);
pI::IntValue puttext_R (puttext_in[4]);
pI::IntValue puttext_G (puttext_in[5]);
pI::IntValue puttext_B (puttext_in[6]);
pI::BoolValue puttext_do_draw (puttext_in[7]);
puttext_in[0] = drawRectDepth_in[0];
puttext_R.SetData (255);
puttext_G.SetData (255);
puttext_B.SetData (255);
puttext_do_draw.SetData (pI_TRUE);
puttext_x.SetData (285);
puttext_y.SetData (272);
// Executes getImageAndDepth and setTiltAndLED a first time to initialize Kinect.
getImageAndDepth->Execute (getImageAndDepth_in, getImageAndDepth_out);
pI::IntValue newTilt (setTiltAndLED_in[0]);
pI::IntValue newLED (setTiltAndLED_in[1]);
newTilt.SetData (0); // Tilt angle 0 degree
newLED.SetData (1); // LED status Green
setTiltAndLED->Execute (setTiltAndLED_in, setTiltAndLED_out);
Poco::Timestamp now;
bool doLoop = true;
bool showDistance = false;
try {
while (doLoop) {
while (!kbhit()) {
// Get Depth and RGB data, ...
getImageAndDepth->Execute (getImageAndDepth_in, getImageAndDepth_out);
// ... apply depth to color image conversion, ...
applyRGB->Execute (applyRGB_in, applyRGB_out);
// ... draw a white rectangle into the center of both image and map, ...
if (showDistance) {
drawRectImage->Execute (drawRectImage_in, drawRectImage_out);
drawRectDepth->Execute (drawRectDepth_in, drawRectDepth_out);
}
// ... and perhaps the result of distance measurement, ...
double avgDistance = 0.;
if (showDistance) {
pI::ShortMatrix depth (getImageAndDepth_out[1]);
double sumDistance = 0.;
int sumCount = 0;
for (int j = 230; j <= 250; ++j) {
for (int i = 310; i <= 330; ++i) {
int dep = depth.GetData (j, i);
if (dep < 2047) {
sumDistance += RawDepthToMeters (dep);
++sumCount;
}
}
}
if (sumCount > 0) {
avgDistance = sumDistance / (double) sumCount;
}
char str[32];
sprintf (str, "%.2f m", avgDistance);
puttext_text.SetData (str);
puttext->Execute (puttext_in, puttext_out);
}
// ... and display both the image and the depth map.
displayImage->Execute (displayImage_in, displayImage_out);
displayDepth->Execute (displayDepth_in, displayDepth_out);
std::cout << "Tilt " << (int) newTilt.GetData();
std::cout << "; " << (1000000. / now.elapsed()) << " frames/sec \r";
now.update();
}
int ch = getch();
switch (ch) {
case 'q':
case 'Q':
case 27 /* ESC */:
// Terminates
newTilt.SetData (0); // Tilt angle 0 degree
newLED.SetData (0); // LED status off
setTiltAndLED->Execute (setTiltAndLED_in, setTiltAndLED_out);
doLoop = false;
break;
case 'm':
case 'M': // Measurement mode
showDistance = !showDistance;
break;
case 's':
case 'S': { // Takes snapshot
std::string dateTime = Poco::DateTimeFormatter::format (Poco::LocalDateTime(), "%Y-%m-%d_%H-%M-%S");
std::string name = std::string ("KinectImage_") + dateTime + std::string (".png");
fileName.SetData (const_cast<char*> (name.c_str()));
saveImage->Execute (saveImage_in);
std::cout << "Saved " << name << std::endl;
// HACK Experimental: saves PCL point cloud
pI::ShortMatrix depth (getImageAndDepth_out[1]);
int width = depth.GetCols();
int height = depth.GetRows();
pcl::PointCloud<pcl::PointXYZ> cloud;
for (int j = 0; j < height; ++j) {
for (int i = 0; i < width; ++i) {
int dep = depth.GetData (j, i);
if (dep < 2048) {
cloud.push_back (DepthToWorld (i, j, dep));
}
}
}
name = std::string ("PointCloud_") + dateTime + std::string (".pcd");
pcl::io::savePCDFileASCII (name, cloud);
std::cout << "Saved " << name << std::endl;
}
break;
case '+':
case 'u':
case 'U':
//std::cout << std::endl << "Tilt: " << newTilt.GetData();
newTilt.SetData (newTilt.GetData() + 1);
//std::cout << " -> " << newTilt.GetData() << std::endl;
setTiltAndLED->Execute (setTiltAndLED_in, setTiltAndLED_out);
break;
case '-':
case 'd':
case 'D':
//std::cout << std::endl << "Tilt: " << newTilt.GetData();
newTilt.SetData (newTilt.GetData() - 1);
//std::cout << " -> " << newTilt.GetData() << std::endl;
setTiltAndLED->Execute (setTiltAndLED_in, setTiltAndLED_out);
break;
default:
break;
}
}
} catch (pI::exception::ExecutionException& e) {
std::cerr << e.what() << std::endl;
}
}
Color::operator rgb_color() const
{
return make_color(red(), green(), blue(), alpha());
}
int main(int argc, char* argv[]) {
if (argc < 2) {
std::cout << "Usage: simpleBD h" << std::endl;
exit(-1);
}
const unsigned int num_particles = 1;
const double h_in = atof(argv[1]);
const double end_time = 1.0;
//const double dt = 0.0001;
const double dt = h_in*h_in/PI;
/*
* Create Compartments (a structured grid from r = (0,0,0) to r = (1,0.1,0.1). resolution = h = h_in for all dimensions)
*/
StructuredGrid grid(Vect3d(0.25-h_in,0,0), Vect3d(0.75+h_in,1,1), Vect3d(h_in,h_in,h_in));
/*
* Create "red" species with D=1. Fill with a uniform distribution of particles
*/
Species red(1, grid);
/*
* create diffusion operator and apply to red species
*/
Diffusion bd; bd.add_species(red);
/*
* Create simulation boundary planes
*/
xplane xlow(0,1),xhigh(1,-1);
yplane ylow(0,1),yhigh(1,-1);
zplane zlow(0,1),zhigh(1,-1);
xplane couple_C_to_M_1(0.25,-1);
xplane couple_C_to_M_2(0.75,1);
xplane first_line_of_compartments(0.25-0.5*h_in,1);
xplane second_line_of_compartments(0.25+0.5*h_in,1);
xplane second_last_line_of_compartments(0.75-0.5*h_in,1);
xplane last_line_of_compartments(0.75+0.5*h_in,1);
/*
* Create jump (periodic) boundaries
*/
JumpBoundaryWithCorrection<xplane> jb1(xlow,xhigh-xlow); jb1.add_species(red);
JumpBoundary<yplane> jb2(ylow,yhigh-ylow); jb2.add_species(red);
JumpBoundary<zplane> jb3(zlow,zhigh-zlow); jb3.add_species(red);
JumpBoundary<yplane> jb4(yhigh,ylow-yhigh); jb4.add_species(red);
JumpBoundary<zplane> jb5(zhigh,zlow-zhigh); jb5.add_species(red);
/*
* Create reflective boundary
*/
ReflectiveBoundary<xplane> rb(xhigh); rb.add_species(red);
/*
* create Next Subvolume Method operator. The constructor takes the compartment grid as input
*/
NextSubvolumeMethod nsm(red.grid);
// add diffusion equations to nsm
//nsm.add_diffusion(red);
nsm.add_diffusion(red, red.D/pow(h_in,2));
/*
* Setup reactions for coupling
*/
// nsm.remove_diffusion_between(red, second_line_of_compartments, first_line_of_compartments);
// nsm.remove_diffusion_between(red, second_last_line_of_compartments, last_line_of_compartments);
// nsm.add_diffusion_between(red, (2.0*h_in/sqrt(PI*red.D*dt))*(red.D/pow(h_in,2)), second_line_of_compartments, first_line_of_compartments);
// nsm.add_diffusion_between(red, (2.0*h_in/sqrt(PI*red.D*dt))*(red.D/pow(h_in,2)), second_last_line_of_compartments, last_line_of_compartments);
nsm.add_diffusion_between(red,red.D/pow(h_in,2),ylow, yhigh);
nsm.add_diffusion_between(red,red.D/pow(h_in,2),yhigh, ylow);
nsm.add_diffusion_between(red,red.D/pow(h_in,2),zlow, zhigh);
nsm.add_diffusion_between(red,red.D/pow(h_in,2),zhigh, zlow);
// nsm.clear_reactions(first_line_of_compartments);
// nsm.clear_reactions(last_line_of_compartments);
nsm.list_reactions();
/*
* Create coupling boundaries between compartments and free-space
*/
Intersection<xplane,xplane> compartment_volume(couple_C_to_M_1, couple_C_to_M_2);
CouplingBoundary_C_to_M<Intersection<xplane,xplane> > c_to_m(compartment_volume, nsm); c_to_m.add_species(red, dt);
CouplingBoundary_M_to_C<Intersection<xplane,xplane> > m_to_c(compartment_volume, nsm); m_to_c.add_species(red);
std::vector<int> bins_m, bins_c;
const int num_bins = 100;
bins_m.assign(num_bins,0);
bins_c.assign(red.grid.get_cells_along_axes()[0],0);
for (int i = 0; i < 50000; ++i) {
std::cout << "doing iteration " << i << std::endl;
red.fill_uniform(Vect3d(0,0,0),Vect3d(1.0,1,1),num_particles);
// calculate next reaction times for all compartments
nsm.reset_all_priorities();
/*
* Run simulation until end_time with timestep dt
*/
run(red, end_time, dt, nsm, bd, jb1, jb2, jb3, jb4, jb5, rb, m_to_c, c_to_m);
if (red.mols.size() > 0) {
const double scaled_position = (red.mols.r[0][0])*num_bins;
if ((scaled_position < 0) || (scaled_position > num_bins)) {
printf("outside area: position = (%f %f %f)\n",red.mols.r[0][0],red.mols.r[0][1],red.mols.r[0][2]);
}
const int index = int(scaled_position);
bins_m[index]++;
red.mols.delete_molecule(0);
}
const int nc = red.grid.size();
for(int i = 0; i < nc; i++) {
Vect3i cell_indices = red.grid.get_cell_indicies(i);
bins_c[cell_indices[0]] += red.copy_numbers[i];
red.copy_numbers[i] = 0;
}
}
std::ofstream f;
f.open(("output/simpleBD_couple_single_mols_"+std::string(argv[1])+".dat").c_str());
f << end_time << ' ';
int count = 0;
BOOST_FOREACH(int x,bins_m) {
f << x <<' ';
count += x;
}
int
dark(int fd, user_info *uin, int t)
{
char x=0,y=0,turn=0,xx,yy,mark=0,color=-1;
char data[100],shown[2]={0,0},result=0,peace=0;
time_t rest,init=time(0);
int key,datac;
// screenline *screen; //for save the scre
data[0]=0;
start(uin);
add_io(fd, 0);
t=t?1:0;
if(!t) { /* 兩邊棋盤資料同步 */
for(yy=0; yy<8; yy++)
strncat(data, chess[yy], 4);
do {
datac=send(fd, data, strlen(data), 0);
} while(datac<0);
}
else {
do {
key=igetkey();
if (key == I_OTHERDATA) {
datac= recv(fd, data, sizeof(data), 0);
if(datac<=0) {add_io(0, 0); return;}
for(yy=0; yy<8; yy++)
strncpy(chess[yy], data+yy*4, 4);
}
} while(key != I_OTHERDATA);
}
move(18,0); clrtoeol();
do {
printtt(19, 56, "%s",t==turn?"自己":"對方");
refresh();
rest=time(0);
do {
if(t==turn) {
show_cursor(y, x, 1);
move(18, 0);
}
key=time(0)-init;
datac=ttt+rest-time(0);
if(datac<1 && turn==t) {
change_record(1);
add_io(0,0); bell();
pressanykey("時間到囉!!..這局輸囉.. :~(");
return 0;
}
else if(datac<1) {
change_record(0);
add_io(0,0); bell();
pressanykey("對方可能睡著啦~ so算你贏了!! ^^Y");
return 0;
}
move(22,34);
prints("%2d:%2d:%2d",key/3600,(key%3600)/60,key%60);
printtt(21,47,"4%d;1m %d:%2d",datac<30?1:4,datac/60,datac%60);
move(b_lines,0); clrtoeol();
prints("[45;37m 說明 [47;31m [←↑↓→/jikl][30m移動 [31m\
[Enter/Space][30m確定 [31m[w][30m傳訊 [31m[p][30m和棋 [31m[q]\
[30m投降 [31m[h][30mHelp [m");
refresh();
key=igetkey();
peace=0;
if(key==I_OTHERDATA) {
bell();
datac= recv(fd, data, sizeof(data), 0);
if(datac<=0) {add_io(0, 0); return;}
if(*data=='P') {
getdata(18, 0, "對方提出和棋的要求..答應和棋嗎?? (y/n) [n]", data, 4, LCECHO, 0);
move(18,0); clrtoeol();
do {
datac=send(fd, data, 1, 0);
} while(datac<0);
if(*data=='y') {
change_record(2);
add_io(0, 0);
pressanykey("雙方握手言和 :)");
return 0;
}
key=0;
}
else if(*data=='Q') {
change_record(0);
add_io(0, 0);
pressanykey("耶耶耶..對方投降了.. ^^Y");
return 0;
}
else {
yy=data[0]-1; xx=data[1]-1;
y=data[2]-1; x=data[3]-1;
if(darked(chess[y][x])) { /* 對方翻棋 */
if(color<0) {
color=nexturn(red(chess[y][x]));
show_color(color);
}
show_chess(y, x, chess[y][x], 0);
chess[y][x]--;
turn=nexturn(turn);
}
else {
if(chess[y][x])
result=show_eaten(chess[y][x],shown[t]++,0);
move_chess(y,x,yy,xx,&mark,&turn);
}
}
}
switch(key) {
case 'h':
{
extern screenline* big_picture;
screenline* screen0 = calloc(t_lines, sizeof(screenline));
memcpy(screen0, big_picture, t_lines * sizeof(screenline));
more("game/dark.help",YEA); //* 改成自己的路徑
memcpy(big_picture, screen0, t_lines * sizeof(screenline));
free(screen0);
redoscr();
}
break;
case 'w':
{
extern screenline* big_picture;
screenline* screen0 = calloc(2, sizeof(screenline));
memcpy(screen0, big_picture, 2 * sizeof(screenline));
my_write(uin->pid, "傳訊息: ");
memcpy(big_picture, screen0, 2 * sizeof(screenline));
free(screen0);
redoscr();
}
break;
case 'p':
if(t==turn) {
int keyy;
if(peace) break;
bell();
getdata(18, 0, "真的要和棋嗎?? (y/n) [n]", data, 4, LCECHO, 0);
move(18, 0); clrtoeol();
if(*data!='y') break;
prints("[33;1m詢問對手是否同意和棋..[5m請稍後... :)[m"); refresh();
peace=1;
do {
datac=send(fd, "P", 1, 0);
} while(datac<0);
do {
keyy=igetkey();
if(keyy==I_OTHERDATA) {
datac= recv(fd, data, sizeof(data), 0);
if(datac<=0) {add_io(0, 0); return 0;}
move(18, 0); clrtoeol();
if(*data=='y') {
change_record(2);
add_io(0, 0);
pressanykey("雙方握手言和 :)");
return 0;
}
else
pressanykey("對方不同意和棋.. :~");
}
} while (keyy!=I_OTHERDATA);
}
break;
case 'q':
if(t==turn) {
bell();
getdata(18, 0, "真的要投降嗎?? (y/n) [n]", data, 4, LCECHO, 0);
move(18,0); clrtoeol();
if(*data!='y') break;
do {
datac=send(fd, "Q", 1, 0);
} while(datac<0);
change_record(1);
add_io(0, 0);
pressanykey("啊啊啊..我不行了..投降~ ^^;");
return 0;
}
break;
default:
if(t!=turn) key=0;
break;
}
if(t!=turn) continue;
switch(key) {
case ' ':
case 13:
if(!mark && darked(chess[y][x])) { /* 翻棋 */
if(color<0) {
color=red(chess[y][x]);
show_color(color);
}
show_chess(y, x, chess[y][x], 0);
chess[y][x]--;
turn=nexturn(turn);
}
else if(mark) { /* 已 mark 的處理 */
if(!chess[y][x] && nexto(x-xx,y-yy)) /* 移動 */
move_chess(y,x,yy,xx,&mark,&turn);
else if(!mychess(y,x,color) && !darked(chess[y][x])) { /* 吃對方的棋子 */
if(nexto2(x-xx,y-yy) && mark==6) { /* mark 起來的棋子不在隔壁(炮) */
char i,tmp=0;
if(x==xx)
for(i=min(y,yy)+1; i<max(y,yy); i++)
tmp+=(chess[i][x]!=0);
else
for(i=min(x,xx)+1; i<max(x,xx); i++)
tmp+=(chess[y][i]!=0);
if(tmp==1 && chess[y][x]) {
result=show_eaten(chess[y][x],shown[nexturn(t)]++,1);
move_chess(y,x,yy,xx,&mark,&turn);
}
else cancel_mark(yy,xx,&mark,&key);
}
else { /* mark 起來的棋子在隔壁 */
if( nexto(x-xx,y-yy) &&
((mark==7 && rank(chess[y][x])==1) || /* 小兵立大功 */
( mark<=rank(chess[y][x]) &&
rank(chess[y][x])-mark!=6 )) ) { /* 將軍怕小兵 */
result=show_eaten(chess[y][x],shown[nexturn(t)]++,1);
move_chess(y,x,yy,xx,&mark,&turn);
}
else cancel_mark(yy,xx,&mark,&key);
}
}
else cancel_mark(yy,xx,&mark,&key); /* 不能移不能吃 */
}
/* mark 起來 */
else if(!mark && chess[y][x] && mychess(y,x,color)) { /* mark 起來 */
show_chess(y, x, chess[y][x], 1);
mark=rank(chess[y][x]);
xx=x; yy=y;
key=0;
}
else key=0;
break;
case 'u':
if(mark) cancel_mark(yy, xx, &mark, &key);
break;
case KEY_LEFT:
case 'j':
show_cursor(y, x, 0);
y=y==0?7:y-1;
break;
case KEY_RIGHT:
case 'l':
show_cursor(y, x, 0);
y=y==7?0:y+1;
break;
case KEY_UP:
case 'i':
show_cursor(y, x, 0);
x=x==0?3:x-1;
break;
case KEY_DOWN:
case 'k':
show_cursor(y, x, 0);
x=x==3?0:x+1;
break;
}
} while(key!=' ' && key!=13 && key!=I_OTHERDATA);
show_cursor(y, x, 0);
if(t==turn) continue; /* turn已經動過..so是相反的值 */
data[0]=yy+1; data[1]=xx+1;
data[2]=y+1; data[3]=x+1;
do {
datac=send(fd, data, 4, 0);
} while(datac<0);
} while(!result);
add_io(0, 0);
if(!turn)
{
FILE *fs;
init=time(0)-init;
if(fs=fopen("log/dark.log","a+")) //* 可改成自己想要的路徑
{
fprintf(fs,"%s win %s %d:%d\n",currutmp->userid,uin->userid,init/60,init%60);
fclose(fs);
}
}
change_record(1);
pressanykey("恭禧你%s了 ^^Y",t==turn?"輸":"贏");
}
