int main(int argc, char *argv[]) try
{
std::cout << "TestDQ\n";
Pose camera = { { 0, 0, 8 }, { 0, 0, 0, 1 } };
bool showaxis = true;
float3 focuspoint(0, 0, 0);
float3 mousevec_prev;
float4 model_orientation(0, 0, 0, 1);
Pose p0 = { { -3, 0, 0 }, { 0, 0, 0, 1 } };
Pose p1 = { { 3, 0, 0 }, { 0, 0, sqrtf(0.5f),sqrtf(0.5f) } };
float dt = 0.01f, t = 0;
Pose *selected = NULL;
std::vector<float4> planes = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { -1, 0, 0, 0 }, { 0, -1, 0, 0 }, { 0, 0, -1, 0 } };
for (auto &p : planes)
p.w = -0.25f;
GLWin glwin("Dual Quaternion Pose Interpolation");
glwin.keyboardfunc = [&](int key, int, int)
{
showaxis = key == 'a' != showaxis;
};
while (glwin.WindowUp())
{
t = t + dt; // advance our global time t is in 0..1
if (t > 1.0f)
t = 0.0f;
Pose pt = dqinterp(p0,p1,t); // And here we show our dual quaterion usage
// some extras to help visualize the axis of rotation, not the best math to get the result, but oh well
float4 aq = qmul(dot(p0.orientation, p1.orientation) < 0 ? -p1.orientation : p1.orientation, qconj(p0.orientation));
float3 axis = normalize(aq.xyz()*(aq.w < 0 ? -1.0f : 1.0f)); // direction of the axis of rotation
float3 axisp = cross(axis, p1.position - p0.position) / 2.0f * sqrtf(1/dot(aq.xyz(),aq.xyz())-1); // origin projected onto the axis of rotation
// user interaction:
float3 ray = qrot(camera.orientation, normalize(glwin.MouseVector)); // for mouse selection
float3 v1 = camera.position + ray*100.0f;
if (!glwin.MouseState) // note that we figure out what is being selected only when the mouse is up
{
selected = NULL;
for (Pose *p : { &p0, &p1 })
{
if (auto h = ConvexHitCheck(planes, *p, camera.position, v1))
{
selected = p;
v1 = h.impact;
}
}
}
else // if (glwin.MouseState)
{
if (selected)
selected->orientation = qmul(VirtualTrackBall(camera.position, selected->position, qrot(camera.orientation, mousevec_prev), qrot(camera.orientation, glwin.MouseVector)), selected->orientation);
else
camera.orientation = qmul(camera.orientation, qconj(VirtualTrackBall(float3(0, 0, 1), float3(0, 0, 0), mousevec_prev, glwin.MouseVector))); // equation is non-typical we are orbiting the camera, not rotating the object
}
camera.position = focuspoint + qzdir(camera.orientation)*magnitude(camera.position - focuspoint);
camera.position -= focuspoint;
camera.position *= powf(1.1f, (float)glwin.mousewheel);
camera.position += focuspoint;
mousevec_prev = glwin.MouseVector;
// Render the scene
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);
glMatrixMode(GL_PROJECTION);
glPushMatrix(); glLoadIdentity();
gluPerspective(glwin.ViewAngle, (double)glwin.Width / glwin.Height, 0.25, 250);
glMatrixMode(GL_MODELVIEW);
glPushMatrix(); glLoadIdentity();
glMultMatrixf(camera.Inverse().Matrix());
glDisable(GL_LIGHTING);
glAxis();
glGridxy(4.0f);
if (showaxis)
{
glPushAttrib(GL_ALL_ATTRIB_BITS);
glLineWidth(3.0f);
glBegin(GL_LINES);
glColor3f(1, 1, 1);
for (auto p : { p0.position, p1.position, pt.position ,axisp})
glVertex3fv(p - axis*0.5f), glVertex3fv(p + axis*0.5f); // note the comma
glEnd();
glPopAttrib();
glColor3f(1, 1, 0);
glBegin(GL_LINES);
glVertex3fv(axisp + axis*dot(axis, p0.position)), glVertex3fv(axisp + axis*dot(axis, p1.position));
glVertex3fv(axisp + axis*dot(axis, p0.position)), glVertex3fv(p0.position);
glVertex3fv(axisp + axis*dot(axis, p1.position)), glVertex3fv(p1.position);
glVertex3fv(axisp + axis*dot(axis, pt.position)), glVertex3fv(pt.position);
glEnd();
}
glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glEnable(GL_COLOR_MATERIAL);
for (auto p : { p0, p1, pt })
glcolorbox(0.25f, p);
glPopMatrix(); //should be currently in modelview mode
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glPopAttrib();// Restore state
glwin.PrintString({ 0, 0 }, "ESC to quit.");
glwin.PrintString({ 0, 1 }, "'a' show axis (%s)", showaxis ? "on" : "off");
glwin.PrintString({ 0, 2 }, "%selected: %s", glwin.MouseState?"S":"s", (selected) ? ((selected==&p0)?"box0":"box1") : "none");
glwin.SwapBuffers();
}
std::cout << "\n";
return 0;
}
catch (std::exception e)
{
std::cerr << e.what() << "\n";
}
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;
}
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;
}
