void FaceRotate(Face & face, const float4 &r)
{
for(auto & v : face.vertex) v = qrot(r,v);
PlaneRotate(face.plane(), r);
face.gu = qrot(r,face.gu);
face.gv = qrot(r,face.gv);
face.ot = qrot(r,face.ot);
}
/*
** ドラッグ中
** マウスのドラッグ中に実行する
*/
void trackballMotion(int x, int y)
{
if (drag) {
double dx, dy, a;
/* マウスポインタの位置のドラッグ開始位置からの変位 */
dx = (x - cx) * sx;
dy = (y - cy) * sy;
//dy = 0;
/* マウスポインタの位置のドラッグ開始位置からの距離 */
a = sqrt(dx * dx + dy * dy);
if (a != 0.0) {
double ar = a * SCALE * 0.5;
double as = sin(ar) / a;
double dq[4] = { cos(ar), dy * as, dx * as, 0.0 };
/* クォータニオンを掛けて回転を合成 */
qmul(tq, dq, cq);
/* クォータニオンから回転の変換行列を求める */
qrot(rt, tq);
}
}
}
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;
}
/*
** トラックボール処理の初期化
** プログラムの初期化処理のところで実行する
*/
void trackballInit(void)
{
/* ドラッグ中ではない */
drag = 0;
/* 単位クォーターニオン */
cq[0] = 1.0;
cq[1] = 0.0;
cq[2] = 0.0;
cq[3] = 0.0;
/* 回転行列の初期化 */
qrot(rt, cq);
}
int main( int argc, char **argv ) {
(void) argc;
(void) argv;
// init
srand((unsigned int)time(NULL)); // might break in 2038
aa_test_ulimit();
cross();
qmul();
qrot();
tfmul();
duqumul();
}
void Trackball::motion(int x, int y)
{
if (dragged) {
double dx, dy, a;
dx = (x - cx) * sx;
dy = (y - cy) * sy;
a = sqrt(dx * dx + dy * dy);
if (a != 0.0)
{
double ar = a * SCALE * 0.5;
double as = sin(ar) / a;
double dq[4] = { cos(ar), dy * as, dx * as, 0.0 };
qmul(tq, dq, cq);
qrot(rt, tq);
}
}
}
int
main(int argc, char **argv)
{
const int seg_num = 5;
const MT_Scalar seg_length = 15;
const float seg_startA[3] = {0,0,0};
const float seg_startB[3] = {0,-20,0};
// create some segments to solve with
// First chain
//////////////
IK_Segment_ExternPtr const segmentsA = new IK_Segment_Extern[seg_num];
IK_Segment_ExternPtr const segmentsB = new IK_Segment_Extern[seg_num];
IK_Segment_ExternPtr seg_it = segmentsA;
IK_Segment_ExternPtr seg_itB = segmentsB;
{
// MT_Quaternion qmat(MT_Vector3(0,0,1),-3.141/2);
MT_Quaternion qmat(MT_Vector3(0,0,1),0);
MT_Matrix3x3 mat(qmat);
seg_it->seg_start[0] = seg_startA[0];
seg_it->seg_start[1] = seg_startA[1];
seg_it->seg_start[2] = seg_startA[2];
float temp[12];
mat.getValue(temp);
seg_it->basis[0] = temp[0];
seg_it->basis[1] = temp[1];
seg_it->basis[2] = temp[2];
seg_it->basis[3] = temp[4];
seg_it->basis[4] = temp[5];
seg_it->basis[5] = temp[6];
seg_it->basis[6] = temp[8];
seg_it->basis[7] = temp[9];
seg_it->basis[8] = temp[10];
seg_it->length = seg_length;
MT_Quaternion q;
q.setEuler(0,0,0);
MT_Matrix3x3 qrot(q);
seg_it->basis_change[0] = 1;
seg_it->basis_change[1] = 0;
seg_it->basis_change[2] = 0;
seg_it->basis_change[3] = 0;
seg_it->basis_change[4] = 1;
seg_it->basis_change[5] = 0;
seg_it->basis_change[6] = 0;
seg_it->basis_change[7] = 0;
seg_it->basis_change[8] = 1;
seg_it ++;
seg_itB->seg_start[0] = seg_startA[0];
seg_itB->seg_start[1] = seg_startA[1];
seg_itB->seg_start[2] = seg_startA[2];
seg_itB->basis[0] = temp[0];
seg_itB->basis[1] = temp[1];
seg_itB->basis[2] = temp[2];
seg_itB->basis[3] = temp[4];
seg_itB->basis[4] = temp[5];
seg_itB->basis[5] = temp[6];
seg_itB->basis[6] = temp[8];
seg_itB->basis[7] = temp[9];
seg_itB->basis[8] = temp[10];
seg_itB->length = seg_length;
seg_itB->basis_change[0] = 1;
seg_itB->basis_change[1] = 0;
seg_itB->basis_change[2] = 0;
seg_itB->basis_change[3] = 0;
seg_itB->basis_change[4] = 1;
seg_itB->basis_change[5] = 0;
seg_itB->basis_change[6] = 0;
seg_itB->basis_change[7] = 0;
seg_itB->basis_change[8] = 1;
seg_itB ++;
}
int i;
for (i=1; i < seg_num; ++i, ++seg_it,++seg_itB) {
MT_Quaternion qmat(MT_Vector3(0,0,1),0.3);
MT_Matrix3x3 mat(qmat);
seg_it->seg_start[0] = 0;
seg_it->seg_start[1] = 0;
seg_it->seg_start[2] = 0;
float temp[12];
mat.getValue(temp);
seg_it->basis[0] = temp[0];
seg_it->basis[1] = temp[1];
seg_it->basis[2] = temp[2];
seg_it->basis[3] = temp[4];
seg_it->basis[4] = temp[5];
seg_it->basis[5] = temp[6];
seg_it->basis[6] = temp[8];
seg_it->basis[7] = temp[9];
seg_it->basis[8] = temp[10];
seg_it->length = seg_length;
MT_Quaternion q;
q.setEuler(0,0,0);
MT_Matrix3x3 qrot(q);
seg_it->basis_change[0] = 1;
seg_it->basis_change[1] = 0;
seg_it->basis_change[2] = 0;
seg_it->basis_change[3] = 0;
seg_it->basis_change[4] = 1;
seg_it->basis_change[5] = 0;
seg_it->basis_change[6] = 0;
seg_it->basis_change[7] = 0;
seg_it->basis_change[8] = 1;
///////////////////////////////
seg_itB->seg_start[0] = 0;
seg_itB->seg_start[1] = 0;
seg_itB->seg_start[2] = 0;
seg_itB->basis[0] = temp[0];
seg_itB->basis[1] = temp[1];
seg_itB->basis[2] = temp[2];
seg_itB->basis[3] = temp[4];
seg_itB->basis[4] = temp[5];
seg_itB->basis[5] = temp[6];
seg_itB->basis[6] = temp[8];
seg_itB->basis[7] = temp[9];
seg_itB->basis[8] = temp[10];
seg_itB->length = seg_length;
seg_itB->basis_change[0] = 1;
seg_itB->basis_change[1] = 0;
seg_itB->basis_change[2] = 0;
seg_itB->basis_change[3] = 0;
seg_itB->basis_change[4] = 1;
seg_itB->basis_change[5] = 0;
seg_itB->basis_change[6] = 0;
seg_itB->basis_change[7] = 0;
seg_itB->basis_change[8] = 1;
}
// create the chains
const int num_chains = 2;
IK_Chain_ExternPtr chains[num_chains];
chains[0] = IK_CreateChain();
chains[1] = IK_CreateChain();
// load segments into chain
IK_LoadChain(chains[0],segmentsA,seg_num);
IK_LoadChain(chains[1],segmentsB,seg_num);
// make and install a mouse handler
MEM_SmartPtr<MyGlutMouseHandler> mouse_handler (MyGlutMouseHandler::New());
GlutMouseManager::Instance()->InstallHandler(mouse_handler);
mouse_handler->SetChain(chains,num_chains);
// make and install a keyhandler
MEM_SmartPtr<MyGlutKeyHandler> key_handler (MyGlutKeyHandler::New());
GlutKeyboardManager::Instance()->InstallHandler(key_handler);
// instantiate the drawing class
MEM_SmartPtr<ChainDrawer> drawer (ChainDrawer::New());
GlutDrawManager::Instance()->InstallDrawer(drawer);
drawer->SetMouseHandler(mouse_handler);
drawer->SetChain(chains,num_chains);
drawer->SetKeyHandler(key_handler);
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
glutCreateWindow("ik");
glutDisplayFunc(GlutDrawManager::Draw);
glutMouseFunc(GlutMouseManager::Mouse);
glutMotionFunc(GlutMouseManager::Motion);
glutKeyboardFunc(GlutKeyboardManager::HandleKeyboard);
init(MT_Vector3(-50,-50,-50),MT_Vector3(50,50,50));
glutMainLoop();
return 0; /* ANSI C requires main to return int. */
}
void TurretShape::_updateNodes(const Point3F& rot)
{
EulerF xRot(rot.x, 0.0f, 0.0f);
EulerF zRot(0.0f, 0.0f, rot.z);
// Set heading
S32 node = mDataBlock->headingNode;
if (node != -1)
{
MatrixF* mat = &mShapeInstance->mNodeTransforms[node];
Point3F defaultPos = mShapeInstance->getShape()->defaultTranslations[node];
Quat16 defaultRot = mShapeInstance->getShape()->defaultRotations[node];
QuatF qrot(zRot);
qrot *= defaultRot.getQuatF();
qrot.setMatrix( mat );
mat->setColumn(3, defaultPos);
}
// Set pitch
node = mDataBlock->pitchNode;
if (node != -1)
{
MatrixF* mat = &mShapeInstance->mNodeTransforms[node];
Point3F defaultPos = mShapeInstance->getShape()->defaultTranslations[node];
Quat16 defaultRot = mShapeInstance->getShape()->defaultRotations[node];
QuatF qrot(xRot);
qrot *= defaultRot.getQuatF();
qrot.setMatrix( mat );
mat->setColumn(3, defaultPos);
}
// Now the mirror direction nodes, if any
for (U32 i=0; i<TurretShapeData::NumMirrorDirectionNodes; ++i)
{
node = mDataBlock->pitchNodes[i];
if (node != -1)
{
MatrixF* mat = &mShapeInstance->mNodeTransforms[node];
Point3F defaultPos = mShapeInstance->getShape()->defaultTranslations[node];
Quat16 defaultRot = mShapeInstance->getShape()->defaultRotations[node];
QuatF qrot(xRot);
qrot *= defaultRot.getQuatF();
qrot.setMatrix( mat );
mat->setColumn(3, defaultPos);
}
node = mDataBlock->headingNodes[i];
if (node != -1)
{
MatrixF* mat = &mShapeInstance->mNodeTransforms[node];
Point3F defaultPos = mShapeInstance->getShape()->defaultTranslations[node];
Quat16 defaultRot = mShapeInstance->getShape()->defaultRotations[node];
QuatF qrot(zRot);
qrot *= defaultRot.getQuatF();
qrot.setMatrix( mat );
mat->setColumn(3, defaultPos);
}
}
mShapeInstance->setDirty(TSShapeInstance::TransformDirty);
}
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 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;
}
// arcball function to do the arcball routine in mouse move
void arcball (rbt &O_frame_in_eye, rbt &O_frame, rbt &new_O_frame, rbt &S_frame, rbt &inv_S_frame,
int g_width, int g_height, double old_x, double old_y, double new_x,
double new_y, int g_manip_object) {
// temp variables
double dx, dy, dz, mag;
bool out_sphere;
coords3 v1, v2, a;
qrot rot, q1, q2;
rbt Q;
// first get the center of the arcball
coords3 center = O_frame_in_eye.translation;
// now set up the projection matrix and variables to hold GetScreenSpaceCircle
matrix4 projmat = MakeProjection(FRUST_FOVY, (float)g_width/(float)g_height, FRUST_NEAR, FRUST_FAR);
coords3 screencenter;
double screenrad;
//get the center and radius in pixles
GetScreenSpaceCircle(center, arcballradius, projmat, g_width, g_height, screencenter, screenrad);
// VECTOR 1 - OLD POINT
dx = old_x - screencenter.x;
dy = old_y - screencenter.y;
mag = (double)sqrt(dx*dx + dy*dy);
out_sphere = (mag > screenrad);
// if the point is out of the sphere, clamp to edge
if(out_sphere == true) {
//normalize dx and dy to clamp dz to 0
dx = (dx / mag);
dy = (dy / mag);
dz = 0.0;
} else {
//solve for z (in X^2 + Y^2 + Z^2 = r^2)
dz = (double)sqrt(screenrad*screenrad - (dx*dx) - (dy*dy));
//normalize
mag = (double)sqrt(dx*dx + dy*dy + dz*dz);
dx = dx / mag;
dy = dy / mag;
dz = dz / mag;
}
v1.SetCoords(dx,dy,dz); //and we have our vector
// VECTOR 2 - NEW POINT
dx = (double)new_x - screencenter.x;
dy = (double)new_y - screencenter.y;
mag = (double)sqrt(dx*dx + dy*dy);
out_sphere = (mag > screenrad);
// if the point is out of the sphere, clamp to edge
if(out_sphere == true) {
//normalize dx and dy to clamp dz to 0
dx = dx / mag;
dy = dy / mag;
dz = 0.0;
} else {
//solve for z (in X^2 + Y^2 + Z^2 = r^2)
dz = (double)sqrt(screenrad*screenrad - (dx*dx) - (dy*dy));
//normalize
mag = (double)sqrt(dx*dx + dy*dy + dz*dz);
dx = dx / mag;
dy = dy / mag;
dz = dz / mag;
}
v2.SetCoords(dx,dy,dz); //and we have our vector
// find the qrot (from class notes it is [0, v1].[0, v0] hence w = 0) and therefore the rbt Q
q1 = qrot(v1.x, v1.y, v1.z, 0.0);
q2 = qrot(v2.x, v2.y, v2.z, 0.0);
if (g_manip_object == 2)
rot = q1*q2;
else
rot = q2 * q1;
Q = rbt(rot);
// CARRY OUT THE O' = SQS^(-1)O ROUTINE
new_O_frame = S_frame * Q * inv_S_frame * O_frame;
}
/**
* @brief Generates a optimised point cloud from a collection of processed keyframes using sparse bundle adjustment.
* @param allFrames A vector containing keyframes which have been completely processed.
* @param outputCloud The cloud generated from the keyframes.
*/
void utils::calculateSBACloud(std::vector<KeyframeContainer>& allFrames,boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >& outputCloud)
{
sba::SysSBA bundleAdjuster;
//set the verbosity of the bundle adjuster
#ifndef NDEBUG
bundleAdjuster.verbose=100;
#else
bundleAdjuster.verbose=0;
#endif
//a list of keypoints that already have been added to the SBA system
std::vector<std::pair<int,uint> > addedPoints; //(frameID,keypointIndex)
//maps frame IDs to frame index in the vector
std::map<int,uint> ID2Ind;
for(uint f=0;f<allFrames.size();++f) ID2Ind[allFrames[f].ID]=f;
//maps the frame vector index to the node vector index
std::map<uint,int> fInd2NInd;
//count the number of 2D/3D-points and camera nodes if in debug mode
#ifndef NDEBUG
uint nrP3D=0;
uint nrP2D=0;
uint nrC=0;
#endif
//add every valid frame as a camera node to the sba system
for(uint f=0;f<allFrames.size();++f)
{
if(!allFrames[f].invalid)
{
//get rotation and translation from the projection matrix
Eigen::Matrix3d rot;
cv::Mat_<double> cvT;
Eigen::Vector4d t;
//get rotation
for(int x=0;x<3;++x) for(int y=0;y<3;++y) rot(x,y)=allFrames[f].projectionMatrix(x,y);
//solve for translation
cv::solve(allFrames[f].projectionMatrix(cv::Range(0,3),cv::Range(0,3)),allFrames[f].projectionMatrix(cv::Range(0,3),cv::Range(3,4)),cvT);
//save translation
for(int x=0;x<3;++x) t(x)=-cvT(x,0);
t(3)=1.0;
//convert rotation to quaternion
Eigen::Quaterniond qrot(rot);
qrot.normalize();
//convert camera calibration matrix
frame_common::CamParams cameraParameters;
cameraParameters.fx=allFrames[f].cameraCalibration(0,0);
cameraParameters.fy=allFrames[f].cameraCalibration(1,1);
cameraParameters.cx=allFrames[f].cameraCalibration(0,2);
cameraParameters.cy=allFrames[f].cameraCalibration(1,2);
cameraParameters.tx=0.0;
//add the frame as a camera node
fInd2NInd[f]=bundleAdjuster.addNode(t,qrot,cameraParameters,false);
#ifndef NDEBUG
++nrC;
#endif
}
else
fInd2NInd[f]=-1;
}
//add the points to the correct nodes
for(uint f=0;f<allFrames.size();++f)
{
if(!allFrames[f].invalid)
{
//add the points
for(uint m=0;m<allFrames[f].matches.size();++m)
{
std::pair<int,uint> myP(allFrames[f].ID,m);
bool goodToAdd=!utils::vectorContainsElement(addedPoints,myP);
//point hasn't been added yet
if(goodToAdd)
{
//check if the point has valid depth information
float depth=allFrames[f].depthImg.image.at<float>(allFrames[f].keypoints[m].pt.y,allFrames[f].keypoints[m].pt.x);
if(isnan(depth)==0)
{
//find the largest completely connected component emanating from this point (clique)
std::vector<std::pair<int,uint> > pointsComplete;
pointsComplete.push_back(std::pair<int,uint>(allFrames[f].ID,m));
//check all matches of the point if they haven't been added yet and their frame is valid
for(uint o=0;o<allFrames[f].matches[m].size();++o)
{
std::pair<int,uint> newElement(allFrames[f].matches[m][o].first.val[0],allFrames[f].matches[m][o].first.val[1]);
if(!utils::vectorContainsElement(addedPoints,newElement) && !allFrames[ID2Ind[newElement.first]].invalid)
pointsComplete.push_back(newElement);
}
//weed out all points that are not completely connected
while(pointsComplete.size()>1)
{
//find the point with the fewest connections (if the component is completely connected all points have the same number of connections)
uint minCorrespondences=pointsComplete.size();
uint worstInd=0;
for(uint i=0;i<pointsComplete.size();++i)
{
//count the number of connections that this point has
uint fInd=ID2Ind[pointsComplete[i].first];
uint myCorr=0;
for(uint q=0;q<allFrames[fInd].matches.size();++q)
for(uint r=0;r<allFrames[fInd].matches[q].size();++r)
{
std::pair<int,uint> tmpElement(allFrames[fInd].matches[q][r].first.val[0],allFrames[fInd].matches[q][r].first.val[1]);
if(utils::vectorContainsElement(pointsComplete,tmpElement))
++myCorr;
}
//save this point if it is the current worst
if(myCorr<minCorrespondences)
{
minCorrespondences=myCorr;
worstInd=i;
}
}
//if the worst point has not the maximal number of connections erase it, else break as all points have the maximal number of connections
if(minCorrespondences<pointsComplete.size()-1)
{
pointsComplete.erase(pointsComplete.begin()+worstInd);
}
else
break;
}
//now pointsComplete contains the clique if the clique has more than one isolated point, write the points to the system
if(pointsComplete.size()>1)
{
//calculate the 3D point and add it to the system
cv::Mat_<double> p2D(2,1,0.0);
p2D(0,0)=allFrames[f].keypoints[m].pt.x;
p2D(1,0)=allFrames[f].keypoints[m].pt.y;
cv::Mat_<double> p3D=reprojectImagePointTo3D(p2D,allFrames[f].cameraCalibration,allFrames[f].projectionMatrix,depth);
Eigen::Vector4d newPoint;
for(uint x=0;x<4;++x) newPoint(x)=p3D(x,0);
int pointIndex=bundleAdjuster.addPoint(newPoint);
#ifndef NDEBUG
++nrP3D;
#endif
//add all image points corresponding to the 3D point to the system
for(uint i=0;i<pointsComplete.size();++i)
{
uint fInd=ID2Ind[pointsComplete[i].first];
Eigen::Vector2d imgPoint;
imgPoint(0)=allFrames[fInd].keypoints[pointsComplete[i].second].pt.x;
imgPoint(1)=allFrames[fInd].keypoints[pointsComplete[i].second].pt.y;
bundleAdjuster.addMonoProj(fInd2NInd[fInd],pointIndex,imgPoint);
#ifndef NDEBUG
++nrP2D;
#endif
}
}
}
}
}
}
}
#ifndef NDEBUG
ROS_INFO("C: %u;3D: %u;2D: %u",nrC,nrP3D,nrP2D);
#endif
//remove bad tracks
bundleAdjuster.calcCost();
bundleAdjuster.removeBad(2);
bundleAdjuster.reduceTracks();
//run 100 iterations of sba
bundleAdjuster.doSBA(100,1e-3, SBA_SPARSE_CHOLESKY);
//save the new projective matrix calculated with sba
for(uint f=0;f<allFrames.size();++f)
if(!allFrames[f].invalid)
for(int x=0;x<3;++x) for(int y=0;y<4;++y) allFrames[f].projectionMatrix(x,y)=bundleAdjuster.nodes[fInd2NInd[f]].w2n(x,y);
//generate the point cloud from the new projection matrices
utils::generateCloud(allFrames,*outputCloud);
}