int APIENTRY WinMain(HINSTANCE hCurrentInst, HINSTANCE hPreviousInst,LPSTR lpszCmdLine, int nCmdShow) // int main(int argc, char *argv[]) { std::cout << "Test tracking\n"; WingMesh box = WingMeshBox({ 0.5, 0.25f, 0.1f }); // our "real world" object used to generate computer vision or depth data input Pose boxpose({ 0, 0, 2 }, normalize(float4( 0.2f, 0.3f, 0.4f, 1.0f ))); RigidBody trackmodel({ AsShape(box) }, { 0, -0.5, 2.25f }); // a tracking model based on the geometry of the real object we are tracking std::vector<RigidBody*> rigidbodies = { &trackmodel }; WingMesh world_slab = WingMeshBox({ -2, -2, -0.75f }, { 2, 2, -0.5f }); // just some ground plane world_geometry GLWin glwin("Tracking single object from depth samples."); InitTex(); glwin.ViewAngle = 60.0f; int2 mouseprev; int animating = 1; float view_dist = 7.0f, view_pitch=20.0f, view_yaw=0; int frame = 0; bool enable_tracking = 0; int sample_resolution = 30; float src_offset = -2.0f; glwin.keyboardfunc = [&](unsigned char key, int x, int y)->void { switch (std::tolower(key)) { case 't': case ' ': enable_tracking = !enable_tracking; break; case 'a': case 's': animating = 1 - animating; break; case '-': case '_': sample_resolution = std::max(sample_resolution - 1, 3); break; case '+': case '=': sample_resolution++; break; case 'q': case 27 : exit(0); break; // ESC case 'x': case 'o': src_offset += 0.5f * ((key == 'X') ? -1.0f : 1.0f); break; case 'r': for (auto &rb : rigidbodies) { rb->position = rb->position_start; rb->orientation = rb->orientation_start; rb->linear_momentum = float3(0, 0, 0); rb->angular_momentum = float3(0, 0, 0); } break; default: std::cout << "unassigned key (" << (int)key << "): '" << key << "'\n"; break; } }; while (glwin.WindowUp()) { frame+=animating; if (glwin.MouseState) // on mouse drag { view_yaw += (glwin.MouseX - mouseprev.x) * 0.3f; // poor man's trackball view_pitch += (glwin.MouseY - mouseprev.y) * 0.3f; } mouseprev = { glwin.MouseX, glwin.MouseY }; view_dist *= powf(1.1f, (float)glwin.mousewheel); boxpose.orientation = normalize(float4(sinf(frame*0.01f),sin(frame*0.035f),sin(frame*0.045f),1.0f)); // animate the source object boxpose.position = float3(sinf(frame*0.01f)*0.75f, cosf(frame*0.01f)*0.75f, boxpose.position.z); std::vector<float3> depthdata; // generated pointcloud for (float y = -1.0f; y <= 1.0f; y += 2.0f/sample_resolution) for (float x = -1.0f; x <= 1.0f; x += 2.0f/sample_resolution) { if (auto hit = ConvexHitCheck(box.faces, boxpose, { 0, 0, 0 }, float3(x, y, 1.0f)*5.0f)) depthdata.push_back(hit.impact); } std::vector<std::pair<float3,float3>> match; if (enable_tracking) { trackmodel.gravscale = 0; trackmodel.damping = 1; std::vector<float4> planesw; for (auto p : box.faces) // should be getting from shape, but oh well planesw.push_back(trackmodel.pose().TransformPlane(p)); std::vector<LimitAngular> angulars; std::vector<LimitLinear> linears; for (auto v : depthdata) { auto plane = planemostbelow(planesw, v); HitInfo hit; auto cp = v - plane.xyz()*dot(plane, float4(v, 1)); // cp is closest point on the plane match.push_back(std::pair<float3, float3>(v, cp)); if (dot(v, plane.xyz()) > 0 && (hit = ConvexHitCheck(planesw, { 0, 0, 0 }, v))) // closest plane is a backface and point is directly behind object linears.push_back(ConstrainAlongDirection(NULL, v, &trackmodel, trackmodel.pose().Inverse()*hit.impact, normalize(v), -50,50)); // push straight backwards else linears.push_back(ConstrainAlongDirection(NULL, v, &trackmodel, trackmodel.pose().Inverse()*cp, plane.xyz(), -50, 50)); } PhysicsUpdate(rigidbodies, linears, angulars, {}); } else { trackmodel.gravscale = 1; trackmodel.damping = 0.1f; PhysicsUpdate(rigidbodies, {}, std::vector<LimitAngular>(0), { &world_slab.verts }); } glPushAttrib(GL_ALL_ATTRIB_BITS); glViewport(0, 0, glwin.Width,glwin.Height); // Set up the viewport glClearColor(0.1f, 0.1f, 0.15f, 1); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glEnable(GL_DEPTH_TEST); // Set up matrices glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); gluPerspective(glwin.ViewAngle, (double)glwin.Width/ glwin.Height, 0.01, 50); glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); gluLookAt(0, -view_dist, 0, 0, 0, 0, 0, 0, 1); glRotatef(view_pitch, 1, 0, 0); glRotatef(view_yaw, 0, 0, 1); glDisable(GL_TEXTURE_2D); glColor3f(1.0f, 0.75f, 0.5f); glPushMatrix(); glTranslatef(src_offset, 0, 0); wmwire(box, boxpose); glPopMatrix(); glColor3f(1.0f, 1.0f, 0.0f); glPointSize(2.0f); glBegin(GL_POINTS); for (auto p : depthdata) glVertex3fv(p); glEnd(); glColor3f(0.7f, 0.0f, 0.0f); glPointSize(1.0f); glBegin(GL_LINES); for (auto p : match) glVertex3fv(p.first), glVertex3fv(p.second); // yeah, no braces {} but note the comma glEnd(); glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(1., 1. / (float)0x10000); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); wmdraw(world_slab); // world_geometry glEnable(GL_TEXTURE_2D); glColor3f(0.5f, 0.5f, 0.5f); for (auto &rb : rigidbodies) rbdraw(rb); glPopAttrib(); // Restore state glMatrixMode(GL_PROJECTION); glPopMatrix(); glMatrixMode(GL_MODELVIEW); glPopMatrix(); glwin.PrintString({ 0, 0 },"ESC/q quits. SPACE to toggle tracking."); glwin.PrintString({ 0, 1 }, "(t)racking %s. (a)nimating %s. depthres %d", (enable_tracking) ? "ON" : "OFF", (animating) ? "ON" : "OFF", sample_resolution); glwin.SwapBuffers(); } std::cout << "\n"; return 0; }
void wmdraw(const WingMesh &m, const Pose &pose) { glPushMatrix(); glMultMatrixf(pose.Matrix()); wmdraw(m); glPopMatrix(); }
int APIENTRY WinMain(HINSTANCE hCurrentInst, HINSTANCE hPreviousInst,LPSTR lpszCmdLine, int nCmdShow) // int main(int argc, char *argv[]) { std::cout << "Test Physics\n"; std::vector<RigidBody*> rigidbodies; rigidbodies.push_back(new RigidBody({ AsShape(WingMeshCube(1)) }, { 1.5f, 0.0f, 1.5f })); rigidbodies.push_back(new RigidBody({ AsShape(WingMeshCube(1)) }, { -1.5f, 0.0f, 1.5f })); rigidbodies.back()->orientation = normalize(float4(0.1f, 0.01f, 0.3f, 1.0f)); auto seesaw = new RigidBody({ AsShape(WingMeshBox( { 3, 0.5f, 0.1f })) }, { 0, -2.5, 0.25f }); rigidbodies.push_back(seesaw); rigidbodies.push_back( new RigidBody({ AsShape(WingMeshCube(0.25f)) }, seesaw->position_start + float3( 2.5f, 0, 0.4f))); rigidbodies.push_back( new RigidBody({ AsShape(WingMeshCube(0.50f)) }, seesaw->position_start + float3(-2.5f, 0, 5.0f))); rbscalemass(rigidbodies.back(), 4.0f); rigidbodies.push_back(new RigidBody({ AsShape(WingMeshBox({1,0.2f,0.2f})),AsShape(WingMeshBox({0.2f,1,0.2f})),AsShape(WingMeshBox({0.2f,0.2f,1})) }, { -1.5f, 0.5f, 7.5f })); for (float z = 5.5f; z < 14.0f; z += 3.0f) rigidbodies.push_back(new RigidBody({ AsShape(WingMeshCube(0.5f)) }, { 0.0f, 0.0f, z })); for (float z = 15.0f; z < 20.0f; z += 3.0f) rigidbodies.push_back(new RigidBody({ AsShape(WingMeshDual(WingMeshCube(0.5f), 0.65f)) }, { 2.0f, -1.0f, z })); WingMesh world_slab = WingMeshBox({ -10, -10, -5 }, { 10, 10, -2 }); // world_geometry GLWin glwin("TestPhys sample"); glwin.ViewAngle = 60.0f; glwin.keyboardfunc = [&](unsigned char key, int x, int y)->void { switch (std::tolower(key)) { case ' ': g_simulate = !g_simulate; break; case 'q': case 27: // ESC exit(0); break; case 'r': for (auto &rb : rigidbodies) { rb->position = rb->position_start; //rb->orientation = rb->orientation_start; // when commented out this provides some variation rb->linear_momentum = float3(0, 0, 0); rb->angular_momentum = float3(0, 0, 0); } seesaw->orientation = { 0, 0, 0, 1 }; break; default: std::cout << "unassigned key (" << (int)key << "): '" << key << "'\n"; break; } }; InitTex(); int2 mouseprev; while (glwin.WindowUp()) { if (glwin.MouseState) // on mouse drag { g_yaw += (glwin.MouseX - mouseprev.x) * 0.3f; // poor man's trackball g_pitch += (glwin.MouseY - mouseprev.y) * 0.3f; } mouseprev = { glwin.MouseX, glwin.MouseY }; if (g_simulate) { std::vector<LimitAngular> angulars; std::vector<LimitLinear> linears; Append(linears , ConstrainPositionNailed(NULL, seesaw->position_start, seesaw, { 0, 0, 0 })); Append(angulars, ConstrainAngularRange(NULL, seesaw, { 0, 0, 0, 1 }, { 0, -20, 0 }, { 0, 20, 0 })); PhysicsUpdate(rigidbodies, linears, angulars, { &world_slab.verts }); } glPushAttrib(GL_ALL_ATTRIB_BITS); glViewport(0, 0, glwin.Width,glwin.Height); // Set up the viewport glClearColor(0.1f, 0.1f, 0.15f, 1); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glEnable(GL_DEPTH_TEST); // Set up matrices glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); gluPerspective(glwin.ViewAngle, (double)glwin.Width/ glwin.Height, 0.01, 50); glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); gluLookAt(0, -8, 5, 0, 0, 0, 0, 0, 1); glRotatef(g_pitch, 1, 0, 0); glRotatef(g_yaw, 0, 0, 1); wmdraw(world_slab); // world_geometry glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(1., 1. / (float)0x10000); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glEnable(GL_TEXTURE_2D); glColor3f(0.5f, 0.5f, 0.5f); for (auto &rb : rigidbodies) rbdraw(rb); glPopAttrib(); // Restore state glMatrixMode(GL_PROJECTION); glPopMatrix(); glMatrixMode(GL_MODELVIEW); glPopMatrix(); glwin.PrintString("ESC/q quits. SPACE to simulate. r to restart", 5, 0); char buf[256]; sprintf_s(buf, "simulation %s", (g_simulate)?"ON":"OFF"); glwin.PrintString(buf, 5, 1); glwin.SwapBuffers(); } std::cout << "\n"; return 0; }