コード例 #1
0
ファイル: jointdrive.cpp プロジェクト: HaiJiaoXinHeng/sandbox
int main(int argc, const char *argv[]) try
{
	std::vector<RigidBody> rbs;
	for (auto const &b : bodysizes)
	{
		auto verts = genboxverts(b);
		auto tris = calchull(verts, 8);
		rbs.push_back(RigidBody({ Shape(verts, tris) }, float3(0, 0, 0)));
	}
	rbscalemass(&rbs[0], 5.0f); // make torso heavier than limb bones
	rbs[0].position.z = 1.0f;  // lift a meter off the ground.
	DXWin mywin("Joint Drive - powered rag doll model", { 800,600 });
	std::vector<Mesh> meshes;
	for (auto &rb : rbs)
	{
		meshes.push_back(MeshSmoothish(rb.shapes[0].verts, rb.shapes[0].tris)); //  1 shape each is known
		rb.damping = 0.8f;   //rb.gravscale = 0;
	}
	for (auto &joint : joints)
	{
		rbs[joint.b0].ignore.push_back(&rbs[joint.b1]);
		rbs[joint.b1].ignore.push_back(&rbs[joint.b0]);
		rbs[joint.b1].position = rbs[joint.b0].pose() * joint.p0 - qrot(rbs[joint.b1].orientation,joint.p1);
	}

	WingMesh ground_wm = WingMeshBox({ -5, -5, -2.0f }, { 5, 5, -1.0f });
	auto ground = MeshFlatShadeTex(ground_wm.verts, WingMeshTris(ground_wm));
	ground.hack = { 0.25f, 0.75f, 0.25f, 1 };
	
	Pose camera = { { 0, -8, 0 }, normalize(float4(0.9f, 0, 0, 1)) };   // where we view the rendered scene from.
	float time = 0;             // our global clock, used to generate the circular animation for upper limbs to follow
	float torquelimit = 38.0f;  // how much torque we let each joint apply each frame

	while (mywin.WindowUp())
	{
		time += 0.06f;

		std::vector<LimitAngular> angulars;
		std::vector<LimitLinear>  linears;
		for (auto const &joint : joints)
		{
			Append(linears, ConstrainPositionNailed(&rbs[joint.b0], joint.p0, &rbs[joint.b1], joint.p1));
			Append(angulars, ConstrainAngularDrive(&rbs[joint.b0], &rbs[joint.b1], (float4(0, joint.a*cos(time), joint.a*sin(time), sqrt(1.0f - joint.a*joint.a))), torquelimit));
		}
		PhysicsUpdate(Addresses<RigidBody>(rbs), linears, angulars, { &ground_wm.verts });

		for (unsigned int i = 0; i < rbs.size(); i++)
			meshes[i].pose = rbs[i].pose();

		mywin.RenderScene(camera, Append(Addresses(meshes), std::vector<Mesh*>({ &ground })));
	}
	return 0;
}
catch (std::exception e)
{
	MessageBoxA(GetActiveWindow(), e.what(), "FAIL", 0);
	return -1;
}
コード例 #2
0
ファイル: testtrack.cpp プロジェクト: 340211173/sandbox-1
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;
}
コード例 #3
0
ファイル: testphys.cpp プロジェクト: leonidk/sandbox_hack
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;
}
コード例 #4
0
ファイル: testrig.cpp プロジェクト: HaiJiaoXinHeng/sandbox
int main(int argc, const char *argv[]) try
{
	std::vector<Joint> joints;
	std::vector<RigidBody> rbs;
	std::map<std::string,unsigned int> rbindex;
	auto xml = XMLParseFile("./default_hand.chr");  // replace string with whatever model you want to test.   uses xml variation of John Ratcliff's easy mesh (ezm) file format.
	auto const &skx = xml.child("model").child("skeleton");
	for (auto const &b : skx.children)
	{
		rbindex[b.attribute("name")] = rbs.size();
		auto verts = ArrayImport<float3>(b.child("verts").body);
		auto tris = calchull(verts, verts.size());
		float3 pos = StringTo<float3>(b.attribute("position"));
		int parent = (b.hasAttribute("parent")) ? (int)rbindex[b.attribute("parent")] : -1;
		rbs.push_back(RigidBody({ Shape(verts,tris) }, pos + ((parent>=0)?rbs[parent].position-rbs[parent].com:float3(0,0,0))));
		if (parent>=0)
			joints.push_back({ parent, (int)rbs.size() - 1, pos, float3(0,0,0), StringTo<float3>(b.child("jointlimitmin").body), StringTo<float3>(b.child("jointlimitmax").body) });
	}
	rbscalemass(&rbs[0], 3.0f);
	rbscalemass(&rbs[1], 5.0f);

	DXWin mywin("DX testing articulated rigged model", { 800,600 });
	std::vector<Mesh> meshes;
	for (auto &rb : rbs)
	{
		meshes.push_back(MeshSmoothish(rb.shapes[0].verts, rb.shapes[0].tris)); //  1 shape each is known
		rb.damping = 0.8f;
		//rb.gravscale = 0;
	}
	for (auto &joint : joints)
	{
		rbs[joint.rbi0].ignore.push_back(&rbs[joint.rbi1]);
		rbs[joint.rbi1].ignore.push_back(&rbs[joint.rbi0]);
		joint.p0 -= rbs[joint.rbi0].com;
		joint.p1 -= rbs[joint.rbi1].com;
	}
	for (auto &ja : joints) for (auto &jb : joints) if (ja.rbi0 == jb.rbi0 && ja.rbi1 != jb.rbi1)  // ignore siblings 
	{
		rbs[ja.rbi1].ignore.push_back(&rbs[jb.rbi1]);
		rbs[jb.rbi1].ignore.push_back(&rbs[ja.rbi1]);
	}
	for (auto &ja : joints) for (auto &jb : joints) if (ja.rbi1 == jb.rbi0 )  // ignore grandparents 
	{
		rbs[ja.rbi0].ignore.push_back(&rbs[jb.rbi1]);
		rbs[jb.rbi1].ignore.push_back(&rbs[ja.rbi0]);
	}

	std::vector<float3> groundpoints = { { -5.0f, -5.0f, -5.0f }, { 5.0f, -5.0f, -5.0f }, { 5.0f, 10.0f, -5.0f }, { -5.0f, 10.0f, -5.0f }, { -5.0f, -5.0f, -10.0f }, { 5.0f, -5.0f, -10.0f }, { 5.0f, 10.0f, -10.0f }, { -5.0f, 10.0f, -10.0f } };
	Mesh ground = MeshSmoothish(groundpoints, { { 0, 1, 2 }, { 2, 3,0 } });
	ground.hack = { 1, 1, 0 ,1};
	WingMesh cube_wm = WingMeshCube(0.025f);
	auto mesh_cube = MeshFlatShadeTex(cube_wm.verts, WingMeshTris(cube_wm));   
	mesh_cube.hack = { 0, 1, 0, 1 };

	Pose camera = { { 0, -10, 0 }, normalize(float4(1, 0, 0, 1)) };
	RigidBody *selected = NULL;
	float3 spoint=camera * float3(0,0,-10);
	float3 rbpoint;
	
	struct Pin{ float3 w; RigidBody* rb; float3 p; };
	std::vector<Pin> pins;   
	
	mywin.keyboardfunc = [&](int key, int, int)
	{
		if (key == 'g') for (auto &rb : rbs) rb.gravscale = 1.0f - rb.gravscale;
		if (key == 'p' && selected)
			Append<Pin>(pins, { spoint, selected, rbpoint });
	};

	while (mywin.WindowUp())
	{
		float3 ray = qrot(camera.orientation, normalize(mywin.MouseVector));
		if (!selected)
		{
			for (auto &rb : rbs)
			{
				float3 v1 = camera.position + ray*100.0f;
				if (auto h=ConvexHitCheck(Planes(rb.shapes[0].verts, rb.shapes[0].tris),rb.pose(),camera.position,v1))
				{
					v1 = h.impact;
					selected = &rb;
					spoint = h.impact;
					rbpoint = rb.pose().inverse()*h.impact;
				}
			}
		}
		spoint = camera.position + ray * length(spoint - camera.position)*powf(1.025f, (float)mywin.mousewheel);
		mesh_cube.pose.position = spoint;
		if (!mywin.MouseState)
			selected = NULL;

		std::vector<LimitAngular> angulars;
		std::vector<LimitLinear>  linears;
		for (auto const &joint : joints)
		{
			Append(linears, ConstrainPositionNailed(&rbs[joint.rbi0], joint.p0, &rbs[joint.rbi1], joint.p1));
			Append(angulars, ConstrainAngularRange(&rbs[joint.rbi0], &rbs[joint.rbi1], { 0, 0, 0, 1 }, joint.jointlimitmin, joint.jointlimitmax));
		}
		if (selected)
			Append(linears, ConstrainPositionNailed(NULL, spoint, selected, rbpoint));
		for(auto &p:pins)
			Append(linears, ConstrainPositionNailed(NULL, p.w,p.rb,p.p));
		PhysicsUpdate(Addresses(rbs), linears, angulars, { &groundpoints });

		for (unsigned int i = 0; i < rbs.size(); i++)
		{
			meshes[i].pose = rbs[i].pose();
		}

		mywin.RenderScene(camera, Append(Addresses(meshes),std::vector<Mesh*>({ &ground, &mesh_cube })));
	}
}
catch (std::exception e)
{
	MessageBoxA(GetActiveWindow(), e.what(), "FAIL", 0);
	return -1;
}
コード例 #5
0
ファイル: dphyspush.cpp プロジェクト: melax/dsamples
int main(int argc, char *argv[]) try
{

	physics_driftmax = 0.0025f;

	GLWin glwin("point cloud push interaction");
	RSCam dcam;
	dcam.Init((argc == 2) ? argv[1] : NULL);
	Image<unsigned short> dimage(dcam.dcamera());

	glwin.ViewAngle = dcam.fov().y;
	float viewdist = 2.0f;
	float yaw = 120;
	int mousexold = 0;
	Mesh   mesh;

	bool   pause = false;
	bool   debuglines=false;
	int    center = 0;
	bool   chains = true;
	bool   usehull = false;
	std::vector<RigidBody*> rigidbodies;
	std::vector < std::pair<RigidBody*, RigidBody*>> links;
	for (float x = -0.2f; x < 0.2f; x+= 0.07f)  
		for(float z: {0.350f})
			for (float y = -0.2f; y <= 0.2f; y += 0.07f)
	{
				rigidbodies.push_back(new RigidBody({ AsShape(WingMeshDual(WingMeshCube(0.025f),0.028f)) }, { x,y,z }));
				//rigidbodies.push_back(new RigidBody({ AsShape(WingMeshCube(0.025f)                       ) }, { x,y,z }));
				links.push_back({(y > -0.2f)?rigidbodies[rigidbodies.size() - 2]:NULL , rigidbodies.back()});
	}

	//rigidbodies.push_back(new RigidBody({ AsShape(WingMeshCube(0.05f)) }, { 0,0,0.50f }));

	auto seesaw = new RigidBody({ AsShape(WingMeshBox({ 0.20f, 0.015f,  0.05f })) }, { 0,0,0.45f });
	rigidbodies.push_back(seesaw);

	glwin.keyboardfunc = [&](unsigned char key, int x, int y)->void
	{
		switch (std::tolower(key))
		{
		case 'q': case 27:  exit(0); break;   // 27 is ESC
		case ' ': pause = !pause; break;
		case 'c': chains = !chains; break;
		case 'd': debuglines = !debuglines; break;
		case 'h': usehull = !usehull; break;
		case 'r': for (auto &rb : rigidbodies) { rb->angular_momentum = rb->linear_momentum = float3(0.0f);rb->pose() = { rb->position_start,rb->orientation_start }; }  break;
		default:  std::cout << "unassigned key (" << (int)key << "): '" << key << "'\n";   break;
		}
	};

	if (dcam.dev->supports_option(rs::option::r200_lr_auto_exposure_enabled))
		dcam.dev->set_option(rs::option::r200_lr_auto_exposure_enabled, 1);

	while (glwin.WindowUp())
	{
		if (glwin.MouseState)
		{
			yaw += glwin.mousepos.x - mousexold;
		}
		mousexold = glwin.mousepos.x;
		viewdist *= powf(1.1f, (float)glwin.mousewheel);

		if (!pause)
			dimage = dcam.GetDepth();

		glPushAttrib(GL_ALL_ATTRIB_BITS);
		glViewport(0, 0, glwin.res.x, glwin.res.y);
		glClearColor(0.1f, 0.1f, 0.15f, 1);
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		glMatrixMode(GL_PROJECTION);
		glPushMatrix();
		glLoadIdentity();
		gluPerspective(glwin.ViewAngle, (double)glwin.aspect_ratio(), 0.01f, 50.0f);

		glMatrixMode(GL_MODELVIEW);
		glPushMatrix();
		gluLookAt(0, 0, viewdist, 0, 0, 0, 0, -1, 0);
		glEnable(GL_DEPTH_TEST);

		glTranslatef(0, 0, 0.35f);
		glRotatef(yaw, 0, 1, 0);
		glTranslatef(0, 0, -0.35f);

		std::vector<float3> pts;
		std::vector<float3> outliers;
		std::vector<float3> vpts;
		glDisable(GL_BLEND);

		float2 wrange = { 0.20f,0.60f };


		auto dp_image_c = Transform(dimage, [](unsigned short s) {return byte3((unsigned char)clamp(255 - s / 4, 0, 255)); });
		drawimage(dp_image_c, { 0.78f,0.22f }, { 0.2f,-0.2f }, 3);

		float depth_scale = (dcam.dev) ? dcam.dev->get_depth_scale() : 0.001f;  // to put into meters    // if file assume file is mm


		for (auto p : rect_iteration(dimage.dim())) // p is int2 from 0,0 to w,h of dcam
		{
			float d = dimage.pixel(p) * depth_scale;  // d is in meters, whereas depth[i] is in camera units  mm for R200, .125mm for SR300 ivycam
			if (p.x<5 || p.x> dimage.dim().x - 5 || p.y<5 || p.y>dimage.dim().y - 5) continue;  // crop, seems to be lots of noise at the edges
			if (d > 1.0f)  // just too far
				continue;  
			float3 v = dimage.cam.deprojectz(asfloat2(p), d);
			if (d>wrange.x && d < wrange.y) 
				pts.push_back(v);
			else
				outliers.push_back(v);
		}

		vpts = ObtainVoxelPointCloud(pts, 0.0082f, 8);


		std::vector<std::pair<float3, float3>> lines;
		std::vector<std::pair<float3, float3>> glines;

		if (1)// && pts.size())
		{
			std::vector<LimitLinear>  linears;
			std::vector<LimitAngular> angulars;
			physics_gravity = { 0,  (float) chains,0 };  // ugg y is down

			if(!usehull) for(auto rb:rigidbodies) 
			{
				if (!rb->shapes[0].planes.size())
					rb->shapes[0].planes = Planes(rb->shapes[0].verts, rb->shapes[0].tris);
				auto planes = Transform(rb->shapes[0].planes, [&](float4 p) { return rb->pose().TransformPlane(p);});
				rb->gravscale = (float)chains;
				float separation = FLT_MAX;
				float3 pushpoint = float3(0, 0, 0);  //
				float4 pushplane;
				for (auto p : vpts)
				{
						auto plane = mostabove(planes, p);
						float sep;
						if ((sep = dot(plane, float4(p, 1))) < separation)
						{
							pushpoint = p;
							pushplane = plane;
							separation = sep;
						}
				}
				if (separation > 0.1f)
					continue;
				float3 closestpoint = ProjectOntoPlane(pushplane, pushpoint);
				pushplane = float4({ -pushplane.xyz(), -dot(-pushplane.xyz(),pushpoint) });
				linears.push_back(ConstrainAlongDirection(NULL, pushpoint, rb, rb->pose().inverse()*closestpoint, pushplane.xyz(), 0, 100.0f)); //  FLT_MAX));
				lines.push_back({ closestpoint,pushpoint });
				auto cp=Separated(rb->shapes[0].verts, rb->position, rb->orientation, { pushpoint }, { 0,0,0 }, { 0,0,0,1 }, 1);
				glines.push_back({ cp.p0w, cp.p1w });
			}
			Append(linears, ConstrainPositionNailed(NULL, seesaw->position_start, seesaw, { 0, 0, 0 }));
			Append(angulars, ConstrainAngularRange(NULL, seesaw, { 0, 0, 0, 1 }, { 0, 0,-20 }, { 0, 0,20 }));

			if (chains) for (auto link : links)
				Append(linears, ConstrainPositionNailed(link.first,link.first? float3(0, 0.035f, 0) : link.second->position_start-float3(0, -0.035f, 0) , link.second, { 0,-0.035f,0 }));
			if(!pause)
				
				if(usehull && vpts.size()>5) 
					PhysicsUpdate(rigidbodies, linears, angulars, { &vpts });
				else 
					PhysicsUpdate(rigidbodies, linears, angulars, std::vector<std::vector<float3>*>());
		}


		glColor3f(1, 1, 1);
		glwirefrustumz(dcam.deprojectextents(), { 0.1f,1.0f });  // draw the camera frustum volume

		glPushAttrib(GL_ALL_ATTRIB_BITS);
		glPointSize(1);
		glBegin(GL_POINTS);
		glColor3f(0, 1, 0.5f);
		for (auto p : pts)
			glVertex3fv(p);
		glColor3f(1, 0.15f, 0.15f);
		for (auto p : outliers)
			glVertex3fv(p);
		glEnd();

		glPointSize(3);
		glBegin(GL_POINTS);
		glColor3f(1, 1, 1);
		for (auto p : vpts)  // was: spts
			glVertex3fv(p);
		glEnd();

		glPopAttrib();

		if (debuglines)
		{
			glBegin(GL_LINES);
			glColor3f(0, 1, 1);
			if (0)for (auto line : lines)
				glVertex3fv(line.first), glVertex3fv(line.second);
			glColor3f(1, 1, 0);
			for (auto line : glines)
				glVertex3fv(line.first), glVertex3fv(line.second);
			glEnd();
		}
		if (usehull && vpts.size() > 5)
		{
			auto tris = calchull(vpts, 0);
			glBegin(GL_LINES);
			glColor3f(1, 1, 1);
			for (auto t : tris) for( int i : {0,1,1,2,2,0})
				glVertex3fv(vpts[t[i]]);
			glEnd();

		}
		if (chains)
		{
			glBegin(GL_LINES);
			glColor3f(1, 0, 1);
			for (auto link : links)
			{
				if(link.first) 
					glVertex3fv(link.first->pose()* float3(0, 0, 0)), glVertex3fv(link.first->pose()* float3(0, 0.035f, 0));
				glVertex3fv(link.second->pose()* float3(0, 0, 0)) , glVertex3fv(link.second->pose()* float3(0, -0.035f, 0));
			}
			glEnd();
		}
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		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

		// Restore state
		glPopMatrix();  //should be currently in modelview mode
		glMatrixMode(GL_PROJECTION);
		glPopMatrix();
		glPopAttrib();
		glMatrixMode(GL_MODELVIEW);

		glwin.PrintString({ 0,0 }, "esc to quit.");
		glwin.PrintString({ 0,1 }, "[h] collision %s  ",(usehull)?"hull":"points");
		glwin.SwapBuffers();

	}
	return 0;
}
catch (const char *c)
{
	MessageBox(GetActiveWindow(), c, "FAIL", 0);
}
catch (std::exception e)
{
	MessageBox(GetActiveWindow(), e.what(), "FAIL", 0);
}