├── README.md
├── .gitignore
├── Pinocchio
    ├── CMakeLists.txt
    ├── debugging.h
    ├── Pinocchio.cpp
    ├── Pinocchio.h
    ├── intersector.h
    ├── indexer.cpp
    ├── utils.h
    ├── mesh.h
    ├── graphutils.h
    ├── multilinear.h
    ├── graphutils.cpp
    ├── pinocchioApi.cpp
    ├── attachment.h
    ├── mathutils.h
    ├── hashutils.h
    ├── dtree.h
    ├── pinocchioApi.h
    ├── skeleton.h
    ├── deriv.h
    ├── intersector.cpp
    ├── matrix.cpp
    ├── vecutils.h
    ├── pointprojector.h
    ├── rect.h
    ├── indexer.h
    ├── discretization.cpp
    ├── refinement.cpp
    ├── transform.h
    ├── attachment.cpp
    ├── quaddisttree.h
    └── matrix.h
├── PinocchioUI
    ├── CMakeLists.txt
    ├── processor.h.no
    ├── demoUI.cpp.no
    ├── DisplayMesh.h
    ├── filter.h
    ├── motion.h
    ├── MyWindow.h.no
    ├── defmesh.h
    ├── defmesh.cpp
    ├── filter.cpp
    └── processor.cpp.no
├── std.h
├── main.cpp
├── KinectPuppeteeringViewer.cpp
├── KinectPuppeteeringViewer.h
├── skeleton_56
├── skeleton_75
├── skeleton_21
├── skeleton_67
├── skeleton_34
├── skeleton_41
├── KinectSkeletonRigger.h
├── CMakeLists.txt
├── KinectSkeleton.h
├── temp17.xml
├── KinectPuppeteering.ui
└── KinectSkeletonRigger.cpp


/README.md:
--------------------------------------------------------------------------------
1 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | build/*


--------------------------------------------------------------------------------
/Pinocchio/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | 
 2 | file(GLOB MYSOURCES	
 3 | 	*.cpp *.c)
 4 | file(GLOB MYHEADERS	
 5 | 	*.h *.hpp)
 6 | 
 7 | list(REMOVE_ITEM MYSOURCES main.cpp)
 8 | 
 9 | 
10 | add_library(Pinocchio ${MYSOURCES} ${MYHEADERS})
11 | 
12 | 


--------------------------------------------------------------------------------
/PinocchioUI/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | 
 2 | file(GLOB PinocUI_SOURCES	
 3 | 	*.cpp *.c)
 4 | file(GLOB PinocUI_HEADERS	
 5 | 	*.h *.hpp)
 6 | 
 7 | message(STATUS "PinocUI_SOURCES ${PinocUI_SOURCES}")
 8 | 
 9 | add_library(PinocchioUI ${PinocUI_SOURCES} ${PinocUI_HEADERS})
10 | 
11 | 


--------------------------------------------------------------------------------
/std.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  *  std.h
 3 |  *  KinectPuppeteering
 4 |  *
 5 |  *  Created by Roy Shilkrot on 11/24/12.
 6 |  *  Copyright 2012 MIT. All rights reserved.
 7 |  *
 8 |  */
 9 | 
10 | #include <fstream>
11 | #include <sstream>
12 | #include <string>
13 | #include <vector>
14 | #include <map>
15 | 
16 | #include <Eigen/Eigen>
17 | #include <Eigen/Core>
18 | #include <Eigen/Geometry>
19 | 
20 | #include <boost/shared_ptr.hpp>
21 | #include <boost/algorithm/string.hpp>
22 | 
23 | #include <GLUT/glut.h>
24 | #include <OpenGL/gl.h>
25 | 


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
 1 | #include "ui_KinectPuppeteering.h"
 2 | #include <qapplication.h>
 3 | 
 4 | class ViewerInterface : public QDialog, public Ui::Dialog
 5 | {
 6 | public:
 7 |     ViewerInterface() { setupUi(this); }
 8 | };
 9 | 
10 | 
11 | int main(int argc, char** argv)
12 | {
13 | 	// Read command lines arguments.
14 | 	QApplication application(argc,argv);
15 | 	
16 | 	// Instantiate the viewer.
17 | 	ViewerInterface viewer;
18 | 	
19 | #if QT_VERSION < 0x040000
20 | 	// Set the viewer as the application main widget.
21 | 	application.setMainWidget(&viewer);
22 | #else
23 | 	viewer.setWindowTitle("KinectPuppeteering");
24 | #endif
25 | 	
26 | 	// Make the viewer window visible on screen.
27 | 	viewer.show();
28 | 	
29 | 	// Run main loop.
30 | 	return application.exec();
31 | }
32 | 


--------------------------------------------------------------------------------
/KinectPuppeteeringViewer.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  *  KinectPuppeteeringViewer.cpp
 3 |  *  KinectPuppeteering
 4 |  *
 5 |  *  Created by Roy Shilkrot on 11/24/12.
 6 |  *  Copyright 2012 MIT. All rights reserved.
 7 |  *
 8 |  */
 9 | 
10 | #include "KinectPuppeteeringViewer.h"
11 | 
12 | void KinectPuppeteeringViewer::draw()
13 | {   
14 | 	//some 3D
15 | 	
16 | 	rigger->drawMesh(false);
17 | 	rigger->drawSkeleton();
18 | 	
19 | //    glColor4f(1.0, 0.0, 0.0, 1.0);
20 | //    drawText(20, 20, QString("here will be 3D!"), QFont("Helvetica [Cronyx]", 20));
21 | }
22 | 
23 | void KinectPuppeteeringViewer::init()
24 | {
25 | 	rigger.reset(new KinectSkeletonRigger);
26 | 	
27 |     // Restore previous viewer state.
28 |     restoreStateFromFile(); 
29 | 	
30 |     setAxisIsDrawn(true);
31 | 	
32 |     // Opens help window
33 |  //   help();
34 | }
35 | 
36 | QString KinectPuppeteeringViewer::helpString() const
37 | {
38 |     QString text("<h2>My QGLViewer</h2>");
39 | 	//.... more text?
40 |     return text;
41 | }
42 | 
43 | // Constructor must call the base class constructor.
44 | KinectPuppeteeringViewer::KinectPuppeteeringViewer(QWidget *parent) : QGLViewer(parent)
45 | {
46 |     restoreStateFromFile();
47 | //    help();
48 | }


--------------------------------------------------------------------------------
/Pinocchio/debugging.h:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #ifndef DEBUGGING_H
20 | #define DEBUGGING_H
21 | 
22 | #include "mathutils.h"
23 | 
24 | class Debugging
25 | {
26 | public:
27 |     static ostream &out() { return *outStream; }
28 |     static void PINOCCHIO_API setOutStream(ostream &os) { outStream = &os; }
29 | 
30 | private:
31 |     static ostream *outStream;
32 | };
33 | 
34 | #endif //DEBUGGING_H
35 | 


--------------------------------------------------------------------------------
/PinocchioUI/processor.h.no:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright (c) 2007 Ilya Baran
 3 | 
 4 | Permission is hereby granted, free of charge, to any person obtaining a copy
 5 | of this software and associated documentation files (the "Software"), to deal
 6 | in the Software without restriction, including without limitation the rights
 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 8 | copies of the Software, and to permit persons to whom the Software is
 9 | furnished to do so, subject to the following conditions:
10 | 
11 | The above copyright notice and this permission notice shall be included in
12 | all copies or substantial portions of the Software.
13 | 
14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
20 | THE SOFTWARE.
21 | */
22 | 
23 | #ifndef PROCESSOR_H
24 | #define PROCESSOR_H
25 | 
26 | #include "MyWindow.h"
27 | 
28 | void process(const vector<string> &args, MyWindow *w);
29 | 
30 | #endif //PROCESSOR_H
31 | 


--------------------------------------------------------------------------------
/KinectPuppeteeringViewer.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  *  KinectPuppeteeringViewer.h
 3 |  *  KinectPuppeteering
 4 |  *
 5 |  *  Created by Roy Shilkrot on 11/24/12.
 6 |  *  Copyright 2012 MIT. All rights reserved.
 7 |  *
 8 |  */
 9 | 
10 | #include <QFileDialog>
11 | #include <QGLViewer/qglviewer.h>
12 | 
13 | #include "KinectSkeletonRigger.h"
14 | 
15 | class KinectPuppeteeringViewer : public QGLViewer
16 | {
17 |     Q_OBJECT
18 | 
19 | 	KinectSkeletonRigger::Ptr rigger;
20 | 	
21 | public:
22 |     KinectPuppeteeringViewer(QWidget *parent = 0);
23 | 	
24 |     protected :
25 |     virtual void draw();
26 |     virtual void init();
27 |     virtual QString helpString() const;
28 | 	
29 |     public slots:
30 | 	void LoadMesh() { rigger->LoadMesh(QFileDialog::getOpenFileName(this,tr("Open Mesh"), "", "*.obj").toStdString()); }
31 | 	void LoadSkeleton() { rigger->LoadSkeletonFromText(QFileDialog::getOpenFileName(this,tr("Open Skeleton"), "", "").toStdString()); }
32 | 	void LoadPose() { rigger->LoadPoseFromText(QFileDialog::getOpenFileName(this,tr("Open Skeleton"), "", "").toStdString()); }
33 | 	void AutoRig() { rigger->AutoRig(); }
34 | 	void SaveRig() { rigger->SaveRig(QFileDialog::getSaveFileName(this,tr("Save Rigging"), "rigging.txt", "*.txt").toStdString()); }
35 | 	void LoadRig() { rigger->LoadRig(QFileDialog::getOpenFileName(this,tr("Open Rigging"), "rigging.txt", "*.txt").toStdString()); }
36 | signals:
37 | 	
38 | };


--------------------------------------------------------------------------------
/skeleton_56:
--------------------------------------------------------------------------------
 1 | -------------------------------------------------------------------
 2 | userID: 1
 3 | neck: 
 4 |   start: 313.12, 273.62, 3084.44
 5 |   end: 313.192, 311.359, 3132.97
 6 | left_shoulder: 
 7 |   start: 313.192, 311.359, 3132.97
 8 |   end: 285.776, 311.182, 3133.92
 9 | left_upper_arm: 
10 |   start: 285.776, 311.182, 3133.92
11 |   end: 242.441, 332.144, 3107.53
12 | left_lower_arm: 
13 |   start: 242.441, 332.144, 3107.53
14 |   end: 236.428, 291.278, 2968.02
15 | right_shoulder: 
16 |   start: 313.192, 311.359, 3132.97
17 |   end: 340.624, 311.535, 3132.03
18 | right_upper_arm: 
19 |   start: 340.624, 311.535, 3132.03
20 |   end: 386.222, 326.542, 3070.13
21 | right_lower_arm: 
22 |   start: 386.222, 326.542, 3070.13
23 |   end: 395.378, 285.392, 2959.41
24 | left_upper_torso: 
25 |   start: 285.776, 311.182, 3133.92
26 |   end: 313.12, 348.161, 3176.02
27 | right_upper_torso: 
28 |   start: 340.624, 311.535, 3132.03
29 |   end: 313.12, 348.161, 3176.02
30 | left_lower_torso: 
31 |   start: 313.12, 348.161, 3176.02
32 |   end: 295.494, 383.852, 3219.7
33 | left_upper_leg: 
34 |   start: 295.494, 383.852, 3219.7
35 |   end: 265.141, 355.08, 2769.53
36 | left_lower_leg: 
37 |   start: 265.141, 355.08, 2769.53
38 |   end: 333.454, 409.957, 2805.79
39 | right_lower_torso: 
40 |   start: 313.12, 348.161, 3176.02
41 |   end: 330.613, 384.106, 3218.45
42 | right_upper_leg: 
43 |   start: 330.613, 384.106, 3218.45
44 |   end: 354.013, 346.537, 2767.13
45 | right_lower_leg: 
46 |   start: 354.013, 346.537, 2767.13
47 |   end: 296.192, 414.166, 2739.6
48 | 


--------------------------------------------------------------------------------
/skeleton_75:
--------------------------------------------------------------------------------
 1 | -------------------------------------------------------------------
 2 | userID: 1
 3 | neck: 
 4 |   start: 294.843, 112.503, 3108.63
 5 |   end: 294.714, 150.959, 3191.95
 6 | left_shoulder: 
 7 |   start: 294.714, 150.959, 3191.95
 8 |   end: 268.024, 149.848, 3200.96
 9 | left_upper_arm: 
10 |   start: 268.024, 149.848, 3200.96
11 |   end: 212.961, 148.528, 3185.51
12 | left_lower_arm: 
13 |   start: 212.961, 148.528, 3185.51
14 |   end: 157.094, 136.594, 3139.65
15 | right_shoulder: 
16 |   start: 294.714, 150.959, 3191.95
17 |   end: 321.555, 152.076, 3182.95
18 | right_upper_arm: 
19 |   start: 321.555, 152.076, 3182.95
20 |   end: 380.62, 156.812, 3165
21 | right_lower_arm: 
22 |   start: 380.62, 156.812, 3165
23 |   end: 436.715, 154.028, 3171.55
24 | left_upper_torso: 
25 |   start: 268.024, 149.848, 3200.96
26 |   end: 294.36, 187.959, 3269.23
27 | right_upper_torso: 
28 |   start: 321.555, 152.076, 3182.95
29 |   end: 294.36, 187.959, 3269.23
30 | left_lower_torso: 
31 |   start: 294.36, 187.959, 3269.23
32 |   end: 277.258, 222.425, 3352.44
33 | left_upper_leg: 
34 |   start: 277.258, 222.425, 3352.44
35 |   end: 285.132, 293.427, 3412.89
36 | left_lower_leg: 
37 |   start: 285.132, 293.427, 3412.89
38 |   end: 286.313, 339.316, 3592.84
39 | right_lower_torso: 
40 |   start: 294.36, 187.959, 3269.23
41 |   end: 310.848, 224.079, 3340.58
42 | right_upper_leg: 
43 |   start: 310.848, 224.079, 3340.58
44 |   end: 308.918, 294.159, 3432.31
45 | right_lower_leg: 
46 |   start: 308.918, 294.159, 3432.31
47 |   end: 312.046, 339.689, 3592.26
48 | 


--------------------------------------------------------------------------------
/skeleton_21:
--------------------------------------------------------------------------------
 1 | -------------------------------------------------------------------
 2 | userID: 1
 3 | neck: 
 4 |   start: 287.192, 113.468, 2341.22
 5 |   end: 290.895, 161.928, 2448.89
 6 | left_shoulder: 
 7 |   start: 290.895, 161.928, 2448.89
 8 |   end: 256.426, 167.474, 2416.46
 9 | left_upper_arm: 
10 |   start: 256.426, 167.474, 2416.46
11 |   end: 201.866, 223.262, 2466.2
12 | left_lower_arm: 
13 |   start: 201.866, 223.262, 2466.2
14 |   end: 150.001, 276.55, 2354.97
15 | right_shoulder: 
16 |   start: 290.895, 161.928, 2448.89
17 |   end: 324.462, 156.527, 2481.33
18 | right_upper_arm: 
19 |   start: 324.462, 156.527, 2481.33
20 |   end: 374.663, 202.802, 2633.49
21 | right_lower_arm: 
22 |   start: 374.663, 202.802, 2633.49
23 |   end: 437.085, 228.261, 2652.44
24 | left_upper_torso: 
25 |   start: 256.426, 167.474, 2416.46
26 |   end: 294.844, 206.877, 2552.85
27 | right_upper_torso: 
28 |   start: 324.462, 156.527, 2481.33
29 |   end: 294.844, 206.877, 2552.85
30 | left_lower_torso: 
31 |   start: 294.844, 206.877, 2552.85
32 |   end: 277.753, 252.355, 2635.46
33 | left_upper_leg: 
34 |   start: 277.753, 252.355, 2635.46
35 |   end: 301.624, 338.221, 2707.91
36 | left_lower_leg: 
37 |   start: 301.624, 338.221, 2707.91
38 |   end: 310.575, 404.491, 2792.6
39 | right_lower_torso: 
40 |   start: 294.844, 206.877, 2552.85
41 |   end: 318.885, 244.329, 2678.15
42 | right_upper_leg: 
43 |   start: 318.885, 244.329, 2678.15
44 |   end: 398.14, 292.064, 2586.23
45 | right_lower_leg: 
46 |   start: 398.14, 292.064, 2586.23
47 |   end: 463.813, 338.294, 2602.38
48 | 


--------------------------------------------------------------------------------
/skeleton_67:
--------------------------------------------------------------------------------
 1 | -------------------------------------------------------------------
 2 | userID: 1
 3 | neck: 
 4 |   start: 306.379, 110.114, 2640.21
 5 |   end: 307.281, 155.72, 2703.46
 6 | left_shoulder: 
 7 |   start: 307.281, 155.72, 2703.46
 8 |   end: 276.691, 154.992, 2733.78
 9 | left_upper_arm: 
10 |   start: 276.691, 154.992, 2733.78
11 |   end: 235.661, 155.545, 2793.02
12 | left_lower_arm: 
13 |   start: 235.661, 155.545, 2793.02
14 |   end: 260.007, 107.687, 2660.93
15 | right_shoulder: 
16 |   start: 307.281, 155.72, 2703.46
17 |   end: 338.566, 156.465, 2673.14
18 | right_upper_arm: 
19 |   start: 338.566, 156.465, 2673.14
20 |   end: 391.861, 157.208, 2658.39
21 | right_lower_arm: 
22 |   start: 391.861, 157.208, 2658.39
23 |   end: 351.418, 104.138, 2595.04
24 | left_upper_torso: 
25 |   start: 276.691, 154.992, 2733.78
26 |   end: 308.006, 199.931, 2766.43
27 | right_upper_torso: 
28 |   start: 338.566, 156.465, 2673.14
29 |   end: 308.006, 199.931, 2766.43
30 | left_lower_torso: 
31 |   start: 308.006, 199.931, 2766.43
32 |   end: 289.378, 241.108, 2849.33
33 | left_upper_leg: 
34 |   start: 289.378, 241.108, 2849.33
35 |   end: 297.333, 320.029, 2968.14
36 | left_lower_leg: 
37 |   start: 297.333, 320.029, 2968.14
38 |   end: 294.646, 379.481, 3073.9
39 | right_lower_torso: 
40 |   start: 308.006, 199.931, 2766.43
41 |   end: 328.292, 243.253, 2809.44
42 | right_upper_leg: 
43 |   start: 328.292, 243.253, 2809.44
44 |   end: 327.928, 322.286, 2941.4
45 | right_lower_leg: 
46 |   start: 327.928, 322.286, 2941.4
47 |   end: 334.479, 379.601, 3027.29
48 | 


--------------------------------------------------------------------------------
/skeleton_34:
--------------------------------------------------------------------------------
 1 | -------------------------------------------------------------------
 2 | userID: 1
 3 | neck: 
 4 |   start: 354.276, 115.696, 2430.97
 5 |   end: 346.505, 163.454, 2512.09
 6 | left_shoulder: 
 7 |   start: 346.505, 163.454, 2512.09
 8 |   end: 319.778, 165.492, 2421.24
 9 | left_upper_arm: 
10 |   start: 319.778, 165.492, 2421.24
11 |   end: 302.081, 225.936, 2402.69
12 | left_lower_arm: 
13 |   start: 302.081, 225.936, 2402.69
14 |   end: 343.626, 272.849, 2309.5
15 | right_shoulder: 
16 |   start: 346.505, 163.454, 2512.09
17 |   end: 371.367, 161.558, 2602.94
18 | right_upper_arm: 
19 |   start: 371.367, 161.558, 2602.94
20 |   end: 379.743, 208.448, 2828.59
21 | right_lower_arm: 
22 |   start: 379.743, 208.448, 2828.59
23 |   end: 388.077, 267.194, 2896.03
24 | left_upper_torso: 
25 |   start: 319.778, 165.492, 2421.24
26 |   end: 338.983, 208.878, 2594.71
27 | right_upper_torso: 
28 |   start: 371.367, 161.558, 2602.94
29 |   end: 338.983, 208.878, 2594.71
30 | left_lower_torso: 
31 |   start: 338.983, 208.878, 2594.71
32 |   end: 315.328, 254.748, 2617.57
33 | left_upper_leg: 
34 |   start: 315.328, 254.748, 2617.57
35 |   end: 312.167, 344.785, 2571.47
36 | left_lower_leg: 
37 |   start: 312.167, 344.785, 2571.47
38 |   end: 255.392, 360.487, 2948.63
39 | right_lower_torso: 
40 |   start: 338.983, 208.878, 2594.71
41 |   end: 347.799, 248.389, 2737.11
42 | right_upper_leg: 
43 |   start: 347.799, 248.389, 2737.11
44 |   end: 348.359, 337.973, 2734.66
45 | right_lower_leg: 
46 |   start: 348.359, 337.973, 2734.66
47 |   end: 327.423, 397.251, 2845.41
48 | 


--------------------------------------------------------------------------------
/PinocchioUI/demoUI.cpp.no:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright (c) 2007 Ilya Baran
 3 | 
 4 | Permission is hereby granted, free of charge, to any person obtaining a copy
 5 | of this software and associated documentation files (the "Software"), to deal
 6 | in the Software without restriction, including without limitation the rights
 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 8 | copies of the Software, and to permit persons to whom the Software is
 9 | furnished to do so, subject to the following conditions:
10 | 
11 | The above copyright notice and this permission notice shall be included in
12 | all copies or substantial portions of the Software.
13 | 
14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
20 | THE SOFTWARE.
21 | */
22 | 
23 | #include <FL/Fl.H>
24 | #include "MyWindow.h"
25 | #include "DisplayMesh.h"
26 | #include "processor.h"
27 | 
28 | int main(int argc, char **argv) {
29 |     MyWindow *window = new MyWindow();
30 | 
31 |     vector<string> args;
32 |     for(int i = 0; i < argc; ++i)
33 |         args.push_back(argv[i]);
34 |     process(args, window);
35 | 
36 |     window->show();
37 | 
38 |     return Fl::run();
39 | }
40 | 


--------------------------------------------------------------------------------
/skeleton_41:
--------------------------------------------------------------------------------
 1 | -------------------------------------------------------------------
 2 | Lost user 2
 3 | userID: 1
 4 | neck: 
 5 |   start: 341.111, 205.672, 2758.19
 6 |   end: 343.724, 246.917, 2827.54
 7 | left_shoulder: 
 8 |   start: 343.724, 246.917, 2827.54
 9 |   end: 317.667, 243.358, 2901.91
10 | left_upper_arm: 
11 |   start: 317.667, 243.358, 2901.91
12 |   end: 283.016, 277.606, 2948.81
13 | left_lower_arm: 
14 |   start: 283.016, 277.606, 2948.81
15 |   end: 269.571, 326.458, 2799.06
16 | right_shoulder: 
17 |   start: 343.724, 246.917, 2827.54
18 |   end: 371.188, 250.669, 2753.17
19 | right_upper_arm: 
20 |   start: 371.188, 250.669, 2753.17
21 |   end: 354.884, 301.229, 2696.04
22 | right_lower_arm: 
23 |   start: 354.884, 301.229, 2696.04
24 |   end: 306.156, 331.92, 2718.03
25 | left_upper_torso: 
26 |   start: 317.667, 243.358, 2901.91
27 |   end: 345.862, 286.624, 2897.01
28 | right_upper_torso: 
29 |   start: 371.188, 250.669, 2753.17
30 |   end: 345.862, 286.624, 2897.01
31 | left_lower_torso: 
32 |   start: 345.862, 286.624, 2897.01
33 |   end: 331.336, 320.958, 3015.41
34 | left_upper_leg: 
35 |   start: 331.336, 320.958, 3015.41
36 |   end: 279.721, 301.774, 2697.08
37 | left_lower_leg: 
38 |   start: 279.721, 301.774, 2697.08
39 |   end: 290.268, 396.931, 2878.45
40 | right_lower_torso: 
41 |   start: 345.862, 286.624, 2897.01
42 |   end: 365.022, 328.1, 2917.55
43 | right_upper_leg: 
44 |   start: 365.022, 328.1, 2917.55
45 |   end: 336.818, 399.901, 3012.83
46 | right_lower_leg: 
47 |   start: 363.427, 412.968, 2936.64
48 |   end: 363.405, 493.141, 2937.14
49 | 


--------------------------------------------------------------------------------
/Pinocchio/Pinocchio.cpp:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | // Pinocchio.cpp : Defines the entry point for the DLL application.
20 | 
21 | #ifdef _WIN32
22 | 
23 | #include "windows.h"
24 | #include "Pinocchio.h"
25 | 
26 | #ifdef _MANAGED
27 | #pragma managed(push, off)
28 | #endif
29 | 
30 | BOOL APIENTRY DllMain( HMODULE hModule,
31 |                        DWORD  ul_reason_for_call,
32 |                        LPVOID lpReserved
33 | 					 )
34 | {
35 | 	switch (ul_reason_for_call)
36 | 	{
37 | 	case DLL_PROCESS_ATTACH:
38 | 	case DLL_THREAD_ATTACH:
39 | 	case DLL_THREAD_DETACH:
40 | 	case DLL_PROCESS_DETACH:
41 | 		break;
42 | 	}
43 |     return TRUE;
44 | }
45 | 
46 | #ifdef _MANAGED
47 | #pragma managed(pop)
48 | #endif
49 | 
50 | #endif
51 | 


--------------------------------------------------------------------------------
/Pinocchio/Pinocchio.h:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | // The following ifdef block is the standard way of creating macros which make exporting 
20 | // from a DLL simpler. All files within this DLL are compiled with the PINOCCHIO_EXPORTS
21 | // symbol defined on the command line. this symbol should not be defined on any project
22 | // that uses this DLL. This way any other project whose source files include this file see 
23 | // PINOCCHIO_API functions as being imported from a DLL, whereas this DLL sees symbols
24 | // defined with this macro as being exported.
25 | #ifdef _WIN32
26 | #ifdef PINOCCHIO_EXPORTS
27 | #define PINOCCHIO_API __declspec(dllexport)
28 | #else //PINOCCHIO_EXPORTS
29 | #define PINOCCHIO_API __declspec(dllimport)
30 | #endif //PINOCCHIO_EXPORTS
31 | #else //_WIN32
32 | #define PINOCCHIO_API
33 | #endif //_WIN32
34 | 


--------------------------------------------------------------------------------
/PinocchioUI/DisplayMesh.h:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright (c) 2007 Ilya Baran
 3 | 
 4 | Permission is hereby granted, free of charge, to any person obtaining a copy
 5 | of this software and associated documentation files (the "Software"), to deal
 6 | in the Software without restriction, including without limitation the rights
 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 8 | copies of the Software, and to permit persons to whom the Software is
 9 | furnished to do so, subject to the following conditions:
10 | 
11 | The above copyright notice and this permission notice shall be included in
12 | all copies or substantial portions of the Software.
13 | 
14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
20 | THE SOFTWARE.
21 | */
22 | 
23 | #ifndef DISPLAYMESH_H
24 | #define DISPLAYMESH_H
25 | 
26 | #include "../Pinocchio/mesh.h"
27 | 
28 | class DisplayMesh
29 | {
30 | public:
31 |     virtual ~DisplayMesh() {}
32 | 
33 |     virtual const Mesh &getMesh() = 0;
34 |     virtual vector<Vector3> getSkel() const { return vector<Vector3>(); }
35 | };
36 | 
37 | class StaticDisplayMesh : public DisplayMesh
38 | {
39 | public:
40 |     StaticDisplayMesh(const Mesh &inM) : m(inM) {}
41 | 
42 |     virtual const Mesh &getMesh() { return m; }
43 | private:
44 | 
45 |     Mesh m;
46 | };
47 | 
48 | #endif //DISPLAYMESH_H
49 | 


--------------------------------------------------------------------------------
/Pinocchio/intersector.h:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #ifndef INTERSECTOR_H
20 | #define INTERSECTOR_H
21 | 
22 | #include "mesh.h"
23 | #include "vecutils.h"
24 | 
25 | class PINOCCHIO_API Intersector {
26 | public:
27 |     Intersector() : mesh(NULL) {}
28 |     Intersector(const Mesh &m, const Vector3 &inDir) : mesh(&m), dir(inDir) { init(); }
29 |     
30 |     vector<Vector3> intersect(const Vector3 &pt, vector<int> *outIndices = NULL) const;    
31 |     const Vector3 &getDir() const { return dir; }
32 | private:
33 |     void init();
34 |     void getIndex(const Vector2 &pt, int &x, int &y) const;
35 |     
36 |     const Mesh *mesh;
37 |     Vector3 dir;
38 |     Vector3 v1, v2; //basis
39 |     Rect2 bounds; //within the basis
40 |     
41 |     vector<Vector2> points;
42 |     vector<Vector3> sNormals; //they are scaled for intersection
43 |     vector<vector<int> > triangles;
44 | };
45 | 
46 | #endif //INTERSECTOR_H
47 | 


--------------------------------------------------------------------------------
/Pinocchio/indexer.cpp:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #include "indexer.h"
20 | 
21 | unsigned int interLeaveLookupTable[32768];
22 | 
23 | class LookupTable
24 | {
25 | public:
26 |     LookupTable()
27 |     {
28 |         for(int i = 0; i < 32768; ++i) {
29 |             interLeaveLookupTable[i] = 0;
30 |             for(int k = 0; k < 15; ++k)
31 |                 if(i & (1 << k))
32 |                     interLeaveLookupTable[i] += (1 << (28 - 2 * k));
33 |         }
34 |     }
35 | };
36 | 
37 | static LookupTable lt;
38 | 
39 | unsigned int interLeave3LookupTable[1024];
40 | 
41 | class LookupTable3
42 | {
43 |     public:
44 |         LookupTable3()
45 |         {
46 |             for(int i = 0; i < 1024; ++i) {
47 |                 interLeave3LookupTable[i] = 0;
48 |                 for(int k = 0; k < 10; ++k)
49 |                     if(i & (1 << k))
50 |                         interLeave3LookupTable[i] += (1 << (27 - 3 * k));
51 |             }
52 |         }
53 | };
54 | 
55 | static LookupTable3 lt3;
56 | 


--------------------------------------------------------------------------------
/PinocchioUI/filter.h:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright (c) 2007 Ilya Baran
 3 | 
 4 | Permission is hereby granted, free of charge, to any person obtaining a copy
 5 | of this software and associated documentation files (the "Software"), to deal
 6 | in the Software without restriction, including without limitation the rights
 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 8 | copies of the Software, and to permit persons to whom the Software is
 9 | furnished to do so, subject to the following conditions:
10 | 
11 | The above copyright notice and this permission notice shall be included in
12 | all copies or substantial portions of the Software.
13 | 
14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
20 | THE SOFTWARE.
21 | */
22 | 
23 | #ifndef FILTER_H_INCLUDED
24 | #define FILTER_H_INCLUDED
25 | 
26 | #include "../Pinocchio/matrix.h"
27 | #include "../Pinocchio/vector.h"
28 | #include "../Pinocchio/transform.h"
29 | 
30 | class MotionFilter
31 | {
32 | public:
33 |     MotionFilter(const vector<Vector3> &inJoints, const vector<int> inPrev)
34 |         : joints(inJoints), prev(inPrev) {}
35 | 
36 |     void step(const vector<Transform<> > &transforms, vector<Vector3> feet);
37 | 
38 |     const vector<Transform<> > &getTransforms() const { return curTransforms; }
39 | 
40 | private:
41 |     Matrixn<double> getJac(const vector<Transform<> > &transforms) const;
42 |     void addTranslation();
43 | 
44 |     vector<Vector3> joints;
45 |     vector<int> prev;
46 | 
47 |     Vector3 prevTrans;
48 |     Vectorn<double> prevFeet;
49 |     vector<Transform<> > curTransforms;
50 | };
51 | 
52 | #endif //FILTER_H_INCLUDED
53 | 


--------------------------------------------------------------------------------
/KinectSkeletonRigger.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  *  KinectSkeletonRigger.h
 3 |  *  KinectPuppeteering
 4 |  *
 5 |  *  Created by Roy Shilkrot on 11/25/12.
 6 |  *  Copyright 2012 MIT. All rights reserved.
 7 |  *
 8 |  */
 9 | 
10 | #include "KinectSkeleton.h"
11 | #include "Pinocchio/pinocchioApi.h"
12 | 
13 | 
14 | class KinectSkeletonRigger {
15 | private:
16 | 	//------- skeleton
17 | 	string m_skeletonfile;
18 | 	KinectSkeleton::Ptr m_skel,m_pose;
19 | 	void LoadPose(const std::string& skelFile, KinectSkeleton::Ptr& _pose) {
20 | 		if (skelFile != "") _pose.reset(new KinectSkeleton(skelFile));
21 | 	}
22 | public:
23 | 	void LoadSkeletonFromText(const std::string& skelFile) {
24 | 		m_skeletonfile = skelFile;
25 | 		LoadPose(skelFile,m_skel);
26 | 	}
27 | 	void LoadPoseFromText(const std::string& skelFile) {
28 | 		LoadPose(skelFile,m_pose);
29 | 		ComputeBoneTransforms();
30 | 	}
31 | 	void drawSkeleton() { drawSkeleton(m_skel, true); drawSkeleton(m_pose); drawEmbeddingSkel(); }
32 | 	void drawSkeleton(const KinectSkeleton::Ptr& _skel, bool draw_frames = false);
33 | 	void drawEmbeddingSkel();
34 | 	
35 | private:	
36 | 	//------- mesh
37 | 	string m_meshfile;
38 | 	Mesh m_m, m_display_mesh;
39 | public:
40 | 	void LoadMesh(const std::string& meshFile) {  
41 | 		if(meshFile == "") return;
42 | 		m_meshfile = meshFile;
43 | 		m_m = Mesh(meshFile);
44 | 		if(m_m.vertices.size() == 0) {
45 | 			cout << "Error reading file.  Aborting." << endl;
46 | 			return;
47 | 		}
48 | 		m_m.normalizeBoundingBox();
49 | 		m_m.computeVertexNormals();	
50 | 	}
51 | 	void drawMesh(bool show_original = false);
52 | 	void drawMesh(const Mesh &m, bool flatShading, Vector3 trans);
53 | 
54 | 	
55 | private:
56 | 	//------- rig
57 | 	vector<Vector<double, -1> > my_weights;
58 | 	PinocchioOutput m_po;
59 | 	map<string,float> embedding_scale;
60 | 	vector<Eigen::Affine3f> Ts;
61 | public:
62 | 	void AutoRig();
63 | 	void ComputeBoneTransforms(const EigT3f& globalT = EigT3f::Identity());
64 | 	void SaveRig(const std::string& f);
65 | 	void LoadRig(const std::string& f);
66 | 	
67 | 	typedef boost::shared_ptr<KinectSkeletonRigger> Ptr;
68 | };


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required (VERSION 2.6) 
 2 | project (KinectPuppeteering) 
 3 | 
 4 | set (CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/CMake")
 5 | 
 6 | include(cotire)
 7 | 
 8 | find_package(OpenGL REQUIRED)
 9 | find_package(Qt4 REQUIRED QtGui QtOpenGL QtXml)
10 | find_package(OpenMP REQUIRED)
11 | find_package(boost REQUIRED)
12 | 
13 | find_package(Eigen3 QUIET)
14 | IF(NOT Eigen3_FOUND)
15 | 	include(FindPkgConfig)
16 | 	IF(PKG_CONFIG_FOUND)
17 | 		pkg_check_modules(EIGEN3 REQUIRED eigen3 )
18 | 		include_directories(${EIGEN3_INCLUDE_DIRS})
19 | 	ENDIF()
20 | ENDIF()
21 | 
22 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
23 | 
24 | add_subdirectory(Pinocchio)
25 | add_subdirectory(PinocchioUI)
26 | 
27 | file(GLOB MYSOURCES	
28 | 	*.cpp *.c)
29 | file(GLOB MYHEADERS	
30 | 	*.h *.hpp)
31 | file(GLOB MYUIS	
32 | 	*.ui)
33 | 
34 | ######## Qt stuff ########
35 | ADD_DEFINITIONS(${QT_DEFINITIONS})
36 | include_directories(${QT_INCLUDES})
37 | include(${QT_USE_FILE}) 
38 | 
39 | QT4_WRAP_UI( MY_UI_HDRS ${MYUIS} )
40 | QT4_WRAP_CPP( MY_MOC_SRCS "KinectPuppeteeringViewer.h" )
41 | 
42 | INCLUDE_DIRECTORIES( ${CMAKE_BINARY_DIR} ) #qt wrapping creates some sources
43 | #########################
44 | 
45 | include_directories(Pinocchio PinocchioUI)
46 | 
47 | IF(APPLE)
48 |    include_directories( /usr/local/include/ )
49 |    INCLUDE_DIRECTORIES ( /System/Library/Frameworks )
50 |    FIND_LIBRARY(COCOA_LIBRARY Cocoa)
51 |    FIND_LIBRARY(GLUT_LIBRARY GLUT )
52 |    FIND_LIBRARY(OpenGL_LIBRARY OpenGL )
53 |    MARK_AS_ADVANCED (COCOA_LIBRARY
54 |                      GLUT_LIBRARY
55 |                      OpenGL_LIBRARY)
56 |    SET(OpenGL_LIBS ${COCOA_LIBRARY} ${GLUT_LIBRARY} ${OpenGL_LIBRARY})
57 | ENDIF (APPLE)
58 | 
59 | add_executable(KinectPuppeteering ${MYSOURCES} ${MYHEADERS} ${MY_UI_HDRS} ${MY_MOC_SRCS})
60 | 
61 | target_link_libraries(KinectPuppeteering 
62 | 	Pinocchio 
63 | 	PinocchioUI
64 | 	${QT_LIBRARIES} 
65 | 	${OpenGL_LIBS}
66 | 	QGLViewer
67 | 	)
68 | 
69 | set_target_properties(KinectPuppeteering PROPERTIES COTIRE_CXX_PREFIX_HEADER_INIT "std.h")
70 | 	
71 | cotire(KinectPuppeteering)


--------------------------------------------------------------------------------
/PinocchioUI/motion.h:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright (c) 2007 Ilya Baran
 3 | 
 4 | Permission is hereby granted, free of charge, to any person obtaining a copy
 5 | of this software and associated documentation files (the "Software"), to deal
 6 | in the Software without restriction, including without limitation the rights
 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 8 | copies of the Software, and to permit persons to whom the Software is
 9 | furnished to do so, subject to the following conditions:
10 | 
11 | The above copyright notice and this permission notice shall be included in
12 | all copies or substantial portions of the Software.
13 | 
14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
20 | THE SOFTWARE.
21 | */
22 | 
23 | #ifndef MOTION_H
24 | #define MOTION_H
25 | 
26 | #include "../Pinocchio/transform.h"
27 | 
28 | class Motion
29 | {
30 | public:
31 |     Motion(const string &filename);
32 | 
33 |     bool empty() const { return data.empty(); }
34 |     vector<Transform<> > get() const;
35 |     vector<Vector3> getPose() const;
36 |     vector<Vector3> getRefPose() const { return refPose; }
37 |     double getLegLength() const { return legLength; }
38 |     double getLegWidth() const { return legWidth; }
39 | 
40 |     const vector<vector<Transform<> > > &getData() const { return data; }
41 |     void setFixedFrame(int inFrame) { fixedFrame = inFrame < 0 ? -1 : (int)(inFrame % data.size()); }
42 | private:
43 |     int getFrameIdx() const;
44 |     void readH(istream &strm);
45 |     vector<vector<Transform<> > > data;
46 |     vector<vector<Vector3> > poses;
47 |     vector<Vector3> refPose;
48 |     double legLength;
49 |     double legWidth;
50 |     int fixedFrame;
51 | };
52 | 
53 | #endif
54 | 


--------------------------------------------------------------------------------
/Pinocchio/utils.h:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #ifndef UTILS_H_INCLUDED
20 | #define UTILS_H_INCLUDED
21 | 
22 | #include <istream>
23 | #include <sstream>
24 | 
25 | template <class T>
26 | inline string toString(const T& obj) {
27 |  ostringstream stream;
28 |  stream << obj;
29 |  return stream.str();
30 | }
31 | 
32 | inline vector<string> readWords(istream &stream)
33 | {
34 |     string whitespace = " \n\t\r";
35 |     stream >> noskipws;
36 |     
37 |     char tmp[10000];
38 |     stream.getline(tmp, 9990);
39 |     string line(tmp);
40 | 
41 |     if(line.size() == 0)
42 |         return vector<string>();
43 | 
44 |     while(line[line.size() - 1] == '\\') {
45 |         line[line.size() - 1] = ' ';
46 |         stream.getline(tmp, 9990);
47 |         line = line + string(tmp);
48 |     }
49 |         
50 |     //split the line into words
51 |     vector<string> words;
52 |     string::size_type pos = 0;
53 |     while(pos != string::npos) {
54 |         pos = line.find_first_not_of(whitespace, pos);
55 |         if(pos == string::npos)
56 |             break;
57 |         string::size_type eow = line.find_first_of(whitespace, pos);
58 |         words.push_back(string(line, pos, eow - pos));
59 |         pos = eow;
60 |     }
61 |     
62 |     return words;
63 | }
64 | 
65 | 
66 | #endif //UTILS_H_INCLUDED
67 | 


--------------------------------------------------------------------------------
/KinectSkeleton.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  *  KinectSkeleton.h
 3 |  *  AmitParametricBowl
 4 |  *
 5 |  *  Created by Roy Shilkrot on 11/11/12.
 6 |  *  Copyright 2012 MIT. All rights reserved.
 7 |  *
 8 |  */
 9 | #pragma once
10 | 
11 | #include "Pinocchio/pinocchioApi.h"
12 | 
13 | typedef Eigen::Vector3f EigV3f;
14 | typedef Eigen::Matrix3f EigM3f;
15 | typedef Eigen::Quaternionf EigQuat;
16 | typedef Eigen::Transform<float,3,Eigen::Affine> EigT3f;
17 | inline Vector3 Eig2PinocV3f(const Eigen::Vector3f& eigv) { return Vector3(eigv[0],eigv[1],eigv[2]); }
18 | inline Eigen::Vector3f Pinoc2EigV3f(const Vector3& pinocv) { return EigV3f(pinocv[0],pinocv[1],pinocv[2]); }
19 | 
20 | class KinectSkeleton : public Skeleton
21 | {
22 | private:
23 | 	string m_skel_file;
24 | 	void getStartEnd(ifstream& ifs, double& startx, double& starty, double& startz, double& endx, double& endy, double& endz);
25 | 	void getXMLStartEnd(ifstream& ifs, double& startx, double& starty, double& startz, double& endx, double& endy, double& endz);
26 | 	EigV3f convertKinectDmapTo3D(const EigV3f& v_);
27 | public:
28 | 	map<string,EigV3f> jointV;
29 | 	map<string,EigM3f> referenceFrames;
30 | 	map<int,string> jointIdxToName;
31 | 	EigM3f m_back_frame;
32 | 	double m_skel_scale;
33 | 	EigV3f m_back_offset;
34 | 	map<int,int> forwardJoint;
35 | 	
36 | 	Eigen::RowVectorXf getAsVector(const EigT3f& global = EigT3f::Identity());
37 | 	
38 | 	void glDraw(const Eigen::Transform<float,3,Eigen::Affine>& global = Eigen::Affine3f::Identity(), bool draw_frames = false);
39 | 	
40 | 	void makeFramesForJoints();
41 | 	
42 | 	EigM3f makeFrameForJoint(const std::string& joint, const std::string& prev_joint);
43 | 	
44 | 	void populateSkelFromFile(const std::string& skelFile);
45 | 	void populateSkelFromXMLFile(const std::string& skelFile);
46 | 	void populateSkelFromVector(const Eigen::RowVectorXf& from_vector);
47 | 	
48 | 	void transformJoints(const EigM3f& back_frame, double skel_scale, const EigV3f& back_offset);	
49 | 	void FindExtremeJoints(vector<string>& extremes);
50 | 	
51 | 	void makePinnocSkel();
52 | 	
53 | 	string getSkelFile() { return m_skel_file; } 
54 | 	
55 | 	KinectSkeleton() {};
56 | 	KinectSkeleton(const Eigen::RowVectorXf& from_vector);
57 |     KinectSkeleton(const std::string& skelFile);
58 | 	KinectSkeleton(const std::string& skelFile, const EigM3f& back_frame, double skel_scale, const EigV3f& back_offset);
59 | 	
60 | 	typedef boost::shared_ptr<KinectSkeleton> Ptr;
61 | };


--------------------------------------------------------------------------------
/Pinocchio/mesh.h:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #ifndef MESH_H
20 | #define MESH_H
21 | 
22 | #include <vector>
23 | 
24 | #include "vector.h"
25 | #include "rect.h"
26 | 
27 | struct MeshVertex
28 | {
29 |     MeshVertex() : edge(-1) {}
30 |     
31 |     Vector3 pos;
32 |     Vector3 normal;
33 |     int edge; //an edge such that edge->prev->vertex is this
34 | };
35 | 
36 | struct MeshEdge
37 | {
38 |     MeshEdge() : vertex(-1), prev(-1), twin(-1) {}
39 |     
40 |     int vertex; //the vertex the edge points to--the start vertex is prev->vertex
41 |     int prev; //ccw, next is prev->prev
42 |     int twin;
43 | };
44 | 
45 | class PINOCCHIO_API Mesh {
46 | public:
47 |     Mesh() : scale(1.) {}
48 |     Mesh(const string &file);
49 | 
50 |     bool integrityCheck() const;
51 |     bool isConnected() const; //returns true if the mesh consists of a single connected component
52 |     void computeVertexNormals();
53 |     void normalizeBoundingBox();
54 |     void computeTopology();
55 |     void writeObj(const string &filename) const;
56 |     
57 | private:
58 |     void readObj(istream &strm);
59 |     void readOff(istream &strm);
60 |     void readPly(istream &strm);
61 |     void readGts(istream &strm);
62 |     void readStl(istream &strm);
63 |     void fixDupFaces();
64 |     void sortEdges(); //sort edges so that triplets forming faces are adjacent
65 | 
66 | public: //data
67 |     vector<MeshVertex> vertices;
68 |     vector<MeshEdge> edges; //halfEdges, really
69 | 
70 |     Vector3 toAdd;
71 |     double scale;
72 | };
73 | 
74 | #endif
75 | 


--------------------------------------------------------------------------------
/PinocchioUI/MyWindow.h.no:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright (c) 2007 Ilya Baran
 3 | 
 4 | Permission is hereby granted, free of charge, to any person obtaining a copy
 5 | of this software and associated documentation files (the "Software"), to deal
 6 | in the Software without restriction, including without limitation the rights
 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 8 | copies of the Software, and to permit persons to whom the Software is
 9 | furnished to do so, subject to the following conditions:
10 | 
11 | The above copyright notice and this permission notice shall be included in
12 | all copies or substantial portions of the Software.
13 | 
14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
20 | THE SOFTWARE.
21 | */
22 | 
23 | #ifndef MYWINDOW_H
24 | #define MYWINDOW_H
25 | 
26 | #include <FL/Fl_Gl_Window.H>
27 | #include "../Pinocchio/mesh.h"
28 | #include "../Pinocchio/transform.h"
29 | #include "DisplayMesh.h"
30 | 
31 | struct LineSegment
32 | {
33 |     LineSegment() : thickness(1.) {}
34 |     LineSegment(const Vector3 &inP1, const Vector3 &inP2,
35 |         const Vector3 &inColor = Vector3(1, 1, 1), double inThickness = 1.)
36 |         : p1(inP1), p2(inP2), color(inColor), thickness(inThickness) {}
37 | 
38 |     Vector3 p1, p2, color;
39 |     double thickness;
40 | };
41 | 
42 | class MyWindow : public Fl_Gl_Window
43 | {
44 | public:
45 |     MyWindow();
46 | 
47 |     virtual ~MyWindow() {}
48 |     virtual void draw();
49 |     virtual int handle(int);
50 | 
51 |     void addMesh(DisplayMesh *inM) { meshes.push_back(inM); }
52 |     void addLine(const LineSegment &l) { lines.push_back(l); }
53 |     void clearLines() { lines.clear(); }
54 | 
55 | private:
56 |     bool flatShading, floor, skeleton;
57 |     Transform<> transform;
58 |     vector<DisplayMesh *> meshes;
59 |     vector<LineSegment> lines;
60 | 
61 |     void resetTransform();
62 |     void initGL();
63 |     void drawMesh(const Mesh &m, bool flatShading, Vector3 trans = Vector3());
64 |     void drawFloor();
65 | };
66 | 
67 | #endif //MYWINDOW_H
68 | 


--------------------------------------------------------------------------------
/Pinocchio/graphutils.h:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #ifndef GRAPHUTILS_H
20 | #define GRAPHUTILS_H
21 | 
22 | #include <queue>
23 | #include "vector.h"
24 | 
25 | struct PtGraph
26 | {
27 |     vector<Vector3> verts;
28 |     vector<vector<int> > edges;
29 |     
30 |     bool integrityCheck() const;
31 | };
32 |     
33 | class ShortestPather
34 | {
35 | public:
36 |     ShortestPather(const PtGraph &g, int root);
37 |         
38 |     vector<int> pathFrom(int vtx) const
39 |     {
40 |         vector<int> out(1, vtx);
41 |         while(prev[vtx] >= 0)
42 |             out.push_back(vtx = prev[vtx]);
43 |         return out;
44 |     }
45 |     double distFrom(int vtx) const { return dist[vtx]; }
46 |             
47 | private:
48 |     struct Inf
49 |     {
50 |         Inf(double inDist, int inNode, int inPrev) : dist(inDist), node(inNode), prev(inPrev) {}
51 |         bool operator<(const Inf &inf) const { return dist > inf.dist; }
52 |         double dist;
53 |         int node, prev;
54 |     };
55 |         
56 |     vector<int> prev;
57 |     vector<double> dist;
58 | };
59 | 
60 | class AllShortestPather
61 | {
62 | public:
63 |     AllShortestPather() {}
64 |     
65 |     AllShortestPather(const PtGraph &g)
66 |     {
67 |         for(int i = 0; i < (int)g.verts.size(); ++i)
68 |             paths.push_back(ShortestPather(g, i));
69 |     }
70 |     
71 |     vector<int> path(int from, int to) const { return paths[to].pathFrom(from); }
72 |     double dist(int from, int to) const { return paths[to].distFrom(from); }
73 |     
74 | private:
75 |     vector<ShortestPather> paths;
76 | };
77 | 
78 | 
79 | #endif
80 | 


--------------------------------------------------------------------------------
/Pinocchio/multilinear.h:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #ifndef MULTILINEAR_H_INCLUDED
20 | #define MULTILINEAR_H_INCLUDED
21 | 
22 | #include "rect.h"
23 | 
24 | //multilinear function over unit hypercube
25 | template<class Value, int Dim>
26 | class Multilinear
27 | {
28 | public:
29 |   Multilinear()
30 |   {
31 |     for(int i = 0; i < num; ++i)
32 |       values[i] = Value();
33 |   }
34 | 
35 |   void setValue(int idx, const Value &value) { values[idx] = value; }
36 |   const Value &getValue(int idx) const { return values[idx]; }
37 | 
38 |   template<class Real>
39 |   Real evaluate(const Vector<Real, Dim> &v) const
40 |   {
41 |     Real out(0);
42 |     for(int i = 0; i < num; ++i) {
43 |         Vector<Real, Dim> corner;
44 |         BitComparator<Dim>::assignCorner(i, v, Vector<Real, Dim>(1.) - v, corner);
45 |         Real factor = corner.accumulate(ident<Real>(), multiplies<Real>());
46 |         out += (factor * Real(values[i]));
47 |     }
48 |     return out;
49 |   }
50 | 
51 |   template<class Real>
52 |   Real integrate(const Rect<Real, Dim> &r) const
53 |   {
54 |     return r.isEmpty() ? Real() : evaluate(r.getCenter()) * r.getContent();
55 |   }
56 | 
57 | private:
58 |   Multilinear(const Multilinear &); //noncopyable
59 | 
60 |   template<class Real>
61 |   static Real pos(const Real &r1, const Real &r2)
62 |   {
63 |     if(r1 <= Real(0) && r2 <= Real(0))
64 |       return Real(0);
65 |     if(r1 >= Real(0) && r2 >= Real(0))
66 |       return Real(1);
67 |     if(r2 >= r1)
68 |       return r2 / (r2 - r1);
69 |     return r1 / (r1 - r2);
70 |   }
71 | 
72 | 
73 |   static const int num = (1 << Dim);
74 |   Value values[num];
75 | };
76 | 
77 | #endif //MULTILINEAR_H_INCLUDED
78 | 


--------------------------------------------------------------------------------
/PinocchioUI/defmesh.h:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright (c) 2007 Ilya Baran
 3 | 
 4 | Permission is hereby granted, free of charge, to any person obtaining a copy
 5 | of this software and associated documentation files (the "Software"), to deal
 6 | in the Software without restriction, including without limitation the rights
 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 8 | copies of the Software, and to permit persons to whom the Software is
 9 | furnished to do so, subject to the following conditions:
10 | 
11 | The above copyright notice and this permission notice shall be included in
12 | all copies or substantial portions of the Software.
13 | 
14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
20 | THE SOFTWARE.
21 | */
22 | 
23 | #ifndef DEFMESH_H_INCLUDED
24 | #define DEFMESH_H_INCLUDED
25 | 
26 | #include "../Pinocchio/attachment.h"
27 | #include "filter.h"
28 | #include "DisplayMesh.h"
29 | 
30 | class Motion;
31 | 
32 | class DefMesh : public DisplayMesh
33 | {
34 | public:
35 |     DefMesh(const Mesh inMesh, const Skeleton &inOrigSkel, const vector<Vector3> &inMatch,
36 |             const Attachment &inAttachment, Motion *inMotion = NULL) 
37 |       : origSkel(inOrigSkel), match(inMatch), attachment(inAttachment),
38 |         origMesh(inMesh), motion(inMotion), filter(match, origSkel.fPrev())
39 |     {
40 |         transforms.resize(origSkel.fPrev().size() - 1);
41 |     }
42 | 
43 |     void setMotion(Motion *inMotion) { motion = inMotion; }
44 |     Motion *getMotion() const { return motion; }
45 |     void updateIfHasMotion() const { if(motion) updateMesh(); }
46 | 
47 |     vector<Vector3> getSkel() const;
48 |     const Skeleton &getOrigSkel() const { return origSkel; }
49 | 
50 |     const Attachment &getAttachment() const { return attachment; }
51 | 
52 |     const Mesh &getMesh() { updateMesh(); return curMesh; }
53 | 
54 | private:
55 |     double getLegRatio() const;
56 |     vector<Transform<> > computeTransforms() const;
57 |     void updateMesh() const;
58 | 
59 |     Skeleton origSkel;
60 |     vector<Vector3> match;
61 |     Attachment attachment;
62 |     Mesh origMesh;
63 |     mutable Mesh curMesh;
64 |     vector<Quaternion<> > transforms;
65 |     Motion *motion;
66 | 
67 |     vector<double> footOffsets;
68 |     mutable MotionFilter filter;
69 | };
70 | 
71 | #endif //DEFMESH_H_INCLUDED
72 | 


--------------------------------------------------------------------------------
/Pinocchio/graphutils.cpp:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #include "graphutils.h"
20 | #include "debugging.h"
21 | 
22 | #define CHECK(pred) { if(!(pred)) { Debugging::out() << "Graph integrity error: " #pred << " in line " << __LINE__ << endl; return false; } }
23 | 
24 | bool PtGraph::integrityCheck() const
25 | {
26 |     CHECK(verts.size() == edges.size());
27 |     
28 |     int i, j, k;
29 |     for(i = 0; i < (int)edges.size(); ++i) {
30 |         for(j = 0; j < (int)edges[i].size(); ++j) {
31 |             int cur = edges[i][j];
32 |             CHECK(cur >= 0);
33 |             CHECK(cur < (int)edges.size());
34 |             CHECK(cur != i); //no self edges
35 |             
36 |             vector<int>::const_iterator it = find(edges[cur].begin(), edges[cur].end(), i);
37 |             CHECK(it != edges[cur].end());
38 |             
39 |             for(k = 0; k < j; ++k) //duplicates
40 |                 CHECK(cur != edges[i][k]);
41 |         }
42 |     }
43 |     
44 |     return true;
45 | }
46 | 
47 | ShortestPather::ShortestPather(const PtGraph &g, int root)
48 | {
49 |     int sz = g.verts.size();
50 |     priority_queue<Inf> todo;
51 |     vector<bool> done(sz, false);
52 |     prev.resize(sz, -1);
53 |     dist.resize(sz, -1);
54 |             
55 |     todo.push(Inf(0., root, -1));
56 |     while(!todo.empty())
57 |     {
58 |         Inf cur = todo.top();
59 |         todo.pop();
60 |         if(done[cur.node])
61 |             continue;
62 |         done[cur.node] = true;
63 |         prev[cur.node] = cur.prev;
64 |         dist[cur.node] = cur.dist;
65 |                 
66 |         const vector<int> &e = g.edges[cur.node];
67 |         for(int i = 0; i < (int)e.size(); ++i) {
68 |             if(!done[e[i]]) {
69 |                 double dist = cur.dist + (g.verts[cur.node] - g.verts[e[i]]).length();
70 |                 todo.push(Inf(dist, e[i], cur.node));
71 |             }
72 |         }
73 |     }
74 | }
75 | 


--------------------------------------------------------------------------------
/Pinocchio/pinocchioApi.cpp:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #include "pinocchioApi.h"
20 | #include "debugging.h"
21 | #include <fstream>
22 | 
23 | ostream *Debugging::outStream = new ofstream();
24 | 
25 | PinocchioOutput autorig(const Skeleton &given, const Mesh &m)
26 | {
27 |     int i;
28 |     PinocchioOutput out;
29 | 
30 |     Mesh newMesh = prepareMesh(m);
31 | 
32 |     if(newMesh.vertices.size() == 0)
33 |         return out;
34 | 
35 |     TreeType *distanceField = constructDistanceField(newMesh);
36 | 
37 |     //discretization
38 |     vector<Sphere> medialSurface = sampleMedialSurface(distanceField);
39 | 
40 |     vector<Sphere> spheres = packSpheres(medialSurface);
41 | 
42 |     PtGraph graph = connectSamples(distanceField, spheres);
43 | 
44 |     //discrete embedding
45 |     vector<vector<int> > possibilities = computePossibilities(graph, spheres, given);
46 | 
47 |     //constraints can be set by respecifying possibilities for skeleton joints:
48 |     //to constrain joint i to sphere j, use: possiblities[i] = vector<int>(1, j);
49 | 
50 |     vector<int> embeddingIndices = discreteEmbed(graph, spheres, given, possibilities);
51 | 
52 |     if(embeddingIndices.size() == 0) { //failure
53 |         delete distanceField;
54 |         return out;
55 |     }
56 | 
57 |     vector<Vector3> discreteEmbedding = splitPaths(embeddingIndices, graph, given);
58 | 
59 |     //continuous refinement
60 |     vector<Vector3> medialCenters(medialSurface.size());
61 |     for(i = 0; i < (int)medialSurface.size(); ++i)
62 |         medialCenters[i] = medialSurface[i].center;
63 | 
64 |     out.embedding = refineEmbedding(distanceField, medialCenters, discreteEmbedding, given);
65 | 
66 |     //attachment
67 |     VisTester<TreeType> *tester = new VisTester<TreeType>(distanceField);
68 |     out.attachment = new Attachment(newMesh, given, out.embedding, tester);
69 | 
70 |     //cleanup
71 |     delete tester;
72 |     delete distanceField;
73 | 
74 |     return out;
75 | }
76 | 
77 | 


--------------------------------------------------------------------------------
/Pinocchio/attachment.h:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #ifndef ATTACHMENT_H
20 | #define ATTACHMENT_H
21 | 
22 | #include "mesh.h"
23 | #include "skeleton.h"
24 | #include "transform.h"
25 | 
26 | class VisibilityTester
27 | {
28 | public:
29 |     virtual ~VisibilityTester() {}
30 |     virtual bool canSee(const Vector3 &v1, const Vector3 &v2) const = 0;
31 | };
32 | 
33 | template<class T> class VisTester : public VisibilityTester
34 | {
35 | public:
36 |     VisTester(const T *t) : tree(t) {}
37 | 
38 |     virtual bool canSee(const Vector3 &v1, const Vector3 &v2) const //faster when v2 is farther inside than v1
39 |     {
40 |         const double maxVal = 0.002;
41 |         double atV2 = tree->locate(v2)->evaluate(v2);
42 |         double left = (v2 - v1).length();
43 |         double leftInc = left / 100.;
44 |         Vector3 diff = (v2 - v1) / 100.;
45 |         Vector3 cur = v1 + diff;
46 |         while(left >= 0.) {
47 |             double curDist = tree->locate(cur)->evaluate(cur);
48 |             if(curDist > maxVal)
49 |                 return false;
50 |             //if curDist and atV2 are so negative that distance won't reach above maxVal, return true
51 |             if(curDist + atV2 + left <= maxVal)
52 |                 return true;
53 |             cur += diff;
54 |             left -= leftInc;
55 |         }
56 |         return true;
57 |     }
58 | 
59 | private:
60 |     const T *tree;
61 | };
62 | template<class T> VisibilityTester *makeVisibilityTester(const T *tree) { return new VisTester<T>(tree); } //be sure to delete afterwards
63 | 
64 | class AttachmentPrivate;
65 | 
66 | class PINOCCHIO_API Attachment
67 | {
68 | public:
69 |     Attachment() : a(NULL) {}
70 |     Attachment(const Attachment &);
71 |     Attachment(const Mesh &mesh, const Skeleton &skeleton, const vector<Vector3> &match, const VisibilityTester *tester);
72 |     virtual ~Attachment();
73 | 
74 |     Mesh deform(const Mesh &mesh, const vector<Transform<> > &transforms) const;
75 |     Vector<double, -1> getWeights(int i) const;
76 | private:
77 |     AttachmentPrivate *a;
78 | };
79 | 
80 | #endif
81 | 


--------------------------------------------------------------------------------
/Pinocchio/mathutils.h:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #ifndef MATHUTILS_H_INCLUDED
20 | #define MATHUTILS_H_INCLUDED
21 | 
22 | #include <math.h>
23 | #include <functional>
24 | #include <string>
25 | #include "Pinocchio.h"
26 | 
27 | #ifdef _WIN32
28 | #pragma warning(disable:4355)
29 | #pragma warning(disable:4996)
30 | #pragma warning(disable:4251)
31 | #endif //_WIN32
32 | 
33 | using namespace std;
34 | 
35 | #ifndef M_PI
36 | #define M_PI 3.14159265358979323846
37 | #endif
38 | 
39 | inline int ROUND(double x) { return (int)(x + 0.5); }
40 | inline int SIGN(double x) { return (x > 0.) ? 1 : -1; }
41 | template<class T> T SQR(const T & x) { return x * x; }
42 | template<class T> T CUBE(const T & x) { return x * x * x; }
43 | template<class T> T QUAD(const T & x) { return SQR(SQR(x)); }
44 | 
45 | template <class T> class maximum : public binary_function<T, T, T>
46 | {
47 | public:
48 |     T operator()(const T &a1, const T &a2) const { return max(a1, a2); }
49 | };
50 | 
51 | template <class T> class minimum : public binary_function<T, T, T>
52 | {
53 | public:
54 |     T operator()(const T &a1, const T &a2) const { return min(a1, a2); }
55 | };
56 | 
57 | template <class A1, class A2, class R> class myMult : public binary_function<A1, A2, R>
58 | {
59 | public:
60 |     R operator()(const A1 &a1, const A2 &a2) const { return a1 * a2; }
61 | };
62 | 
63 | template <class A1, class A2, class R> class myDiv : public binary_function<A1, A2, R>
64 | {
65 | public:
66 |     R operator()(const A1 &a1, const A2 &a2) const { return a1 / a2; }
67 | };
68 | 
69 | template <class T> class ident : public unary_function<T, T>
70 | {
71 | public:
72 |     T operator()(const T &v) const { return v; }
73 | };
74 | 
75 | template<class T> class construct
76 | {
77 | public:
78 |     typedef T result_type;
79 |     template<class From> T operator()(const From &x) const { return T(x); }
80 | };
81 | 
82 | #ifdef _WIN32
83 | inline bool operator==(const string &s1, const string &s2) { return s1.compare(s2) == 0; }
84 | inline bool operator<(const string &s1, const string &s2) { return s1.compare(s2) < 0; }
85 | #endif
86 | 
87 | #endif //MATHUTILS_H_INCLUDED
88 | 


--------------------------------------------------------------------------------
/Pinocchio/hashutils.h:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #ifndef HASHUTILS_H
20 | #define HASHUTILS_H
21 | 
22 | #include "mathutils.h"
23 | 
24 | #ifndef _WIN32
25 | #include <ext/hash_map>
26 | #include <ext/hash_set>
27 | 
28 | #define _HASH_NAMESPACE __gnu_cxx
29 | 
30 | using namespace _HASH_NAMESPACE;
31 | 
32 | namespace _HASH_NAMESPACE {
33 |     template<class T1, class T2> struct hash<pair<T1, T2> >
34 |     {
35 |         size_t operator()(const pair<T1, T2> &p) const { return hash<T1>()(p.first) + 37 * hash<T2>()(p.second); }
36 |     };
37 |     
38 |     template<class T> struct hash<T *>
39 |     {
40 |         size_t operator()(T *p) const { return (size_t)p; }
41 |     };
42 | }
43 | 
44 | #define MAKE_HASH(type, code) \
45 |     namespace _HASH_NAMESPACE { \
46 |         template<> struct hash<type> \
47 |         { \
48 |             size_t operator()(const type &p) const { code } \
49 |         }; \
50 |     }
51 | #else //MICROSOFT VC 2005
52 | #include <hash_map>
53 | #include <hash_set>
54 | 
55 | #define _HASH_NAMESPACE stdext
56 | 
57 | using namespace _HASH_NAMESPACE;
58 | 
59 | namespace _HASH_NAMESPACE {
60 |     template<class T> struct hash
61 |     {
62 |         size_t operator()(const T &p) { return hash_compare<T>()(p); }
63 |     };
64 | 
65 |     template<class T1, class T2> struct hash_compare<std::pair<T1, T2> >
66 |     {
67 |         static const size_t bucket_size = 4;
68 |         static const size_t min_buckets = 8;
69 |         size_t operator()(const std::pair<T1, T2> &p) const { return hash_compare<T1>()(p.first) + 37 * hash_compare<T2>()(p.second); }
70 |         bool operator()(const std::pair<T1, T2> &p1, const std::pair<T1, T2> &p2) const { return p1 < p2; }
71 |     };
72 |     
73 |     template<class T> struct hash_compare<T *>
74 |     {
75 |         static const size_t bucket_size = 4;
76 |         static const size_t min_buckets = 8;
77 |         size_t operator()(T *p) const { return (size_t)p; }
78 |         bool operator()(T *p1, T *p2) const { return p1 < p2; }
79 |     };
80 | }
81 | 
82 | #define MAKE_HASH(type, code) \
83 |     namespace _HASH_NAMESPACE { \
84 |         template<> struct hash_compare<type> \
85 |         { \
86 |             static const size_t bucket_size = 4; \
87 |             static const size_t min_buckets = 8; \
88 |             size_t operator()(const type &p) const { code } \
89 |             bool operator()(const type &p1, const type &p2) const { return p1 < p2; } \
90 |         }; \
91 |     }
92 | #endif
93 |    
94 | #endif //HASHUTILS_H
95 | 


--------------------------------------------------------------------------------
/temp17.xml:
--------------------------------------------------------------------------------
  1 | <frame>324
  2 |     <userID>1</userID>
  3 |     <neck>
  4 |         <X0>311.272</X0>
  5 |         <Y0>167.286</Y0>
  6 |         <Z0>2604.07</Z0>
  7 |         <X1>311.706</X1>
  8 |         <Y1>211.052</Y1>
  9 |         <Z1>2659.08</Z1>
 10 |     </neck>
 11 |     <left_shoulder>
 12 |         <X0>311.706</X0>
 13 |         <Y0>211.052</Y0>
 14 |         <Z0>2659.08</Z0>
 15 |         <X1>278.892</X1>
 16 |         <Y1>211.216</Y1>
 17 |         <Z1>2662.81</Z1>
 18 |     </left_shoulder>
 19 |     <left_upper_arm>
 20 |         <X0>278.892</X0>
 21 |         <Y0>211.216</Y0>
 22 |         <Z0>2662.81</Z0>
 23 |         <X1>225.96</X1>
 24 |         <Y1>198.254</Y1>
 25 |         <Z1>2619.43</Z1>
 26 |     </left_upper_arm>
 27 |     <left_lower_arm>
 28 |         <X0>225.96</X0>
 29 |         <Y0>198.254</Y0>
 30 |         <Z0>2619.43</Z0>
 31 |         <X1>240.872</X1>
 32 |         <Y1>145.262</Y1>
 33 |         <Z1>2476.92</Z1>
 34 |     </left_lower_arm>
 35 |     <right_shoulder>
 36 |         <X0>311.706</X0>
 37 |         <Y0>211.052</Y0>
 38 |         <Z0>2659.08</Z0>
 39 |         <X1>344.612</X1>
 40 |         <Y1>210.888</Y1>
 41 |         <Z1>2655.35</Z1>
 42 |     </right_shoulder>
 43 |     <right_upper_arm>
 44 |         <X0>344.612</X0>
 45 |         <Y0>210.888</Y0>
 46 |         <Z0>2655.35</Z0>
 47 |         <X1>402.397</X1>
 48 |         <Y1>202.159</Y1>
 49 |         <Z1>2566.49</Z1>
 50 |     </right_upper_arm>
 51 |     <right_lower_arm>
 52 |         <X0>402.397</X0>
 53 |         <Y0>202.159</Y0>
 54 |         <Z0>2566.49</Z0>
 55 |         <X1>390.89</X1>
 56 |         <Y1>144.899</Y1>
 57 |         <Z1>2442.08</Z1>
 58 |     </right_lower_arm>
 59 |     <left_upper_torso>
 60 |         <X0>278.892</X0>
 61 |         <Y0>211.216</Y0>
 62 |         <Z0>2662.81</Z0>
 63 |         <X1>312.324</X1>
 64 |         <Y1>254.221</Y1>
 65 |         <Z1>2714.49</Z1>
 66 |     </left_upper_torso>
 67 |     <right_upper_torso>
 68 |         <X0>344.612</X0>
 69 |         <Y0>210.888</Y0>
 70 |         <Z0>2655.35</Z0>
 71 |         <X1>312.324</X1>
 72 |         <Y1>254.221</Y1>
 73 |         <Z1>2714.49</Z1>
 74 |     </right_upper_torso>
 75 |     <left_lower_torso>
 76 |         <X0>312.324</X0>
 77 |         <Y0>254.221</Y0>
 78 |         <Z0>2714.49</Z0>
 79 |         <X1>292.967</X1>
 80 |         <Y1>295.691</Y1>
 81 |         <Z1>2772.25</Z1>
 82 |     </left_lower_torso>
 83 |     <left_upper_leg>
 84 |         <X0>292.967</X0>
 85 |         <Y0>295.691</Y0>
 86 |         <Z0>2772.25</Z0>
 87 |         <X1>287.221</X1>
 88 |         <Y1>384.142</Y1>
 89 |         <Z1>2717.24</Z1>
 90 |     </left_upper_leg>
 91 |     <left_lower_leg>
 92 |         <X0>287.221</X0>
 93 |         <Y0>384.142</Y0>
 94 |         <Z0>2717.24</Z0>
 95 |         <X1>279.037</X1>
 96 |         <Y1>454.686</Y1>
 97 |         <Z1>2839.12</Z1>
 98 |     </left_lower_leg>
 99 |     <right_lower_torso>
100 |         <X0>312.324</X0>
101 |         <Y0>254.221</Y0>
102 |         <Z0>2714.49</Z0>
103 |         <X1>332.902</X1>
104 |         <Y1>295.636</Y1>
105 |         <Z1>2767.53</Z1>
106 |     </right_lower_torso>
107 |     <right_upper_leg>
108 |         <X0>332.902</X0>
109 |         <Y0>295.636</Y0>
110 |         <Z0>2767.53</Z0>
111 |         <X1>336.861</X1>
112 |         <Y1>383.24</Y1>
113 |         <Z1>2744.79</Z1>
114 |     </right_upper_leg>
115 |     <right_lower_leg>
116 |         <X0>336.861</X0>
117 |         <Y0>383.24</Y0>
118 |         <Z0>2744.79</Z0>
119 |         <X1>337.03</X1>
120 |         <Y1>452.078</Y1>
121 |         <Z1>2878.51</Z1>
122 |     </right_lower_leg>
123 | </frame>
124 | 


--------------------------------------------------------------------------------
/Pinocchio/dtree.h:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #ifndef DTREE_H
 20 | #define DTREE_H
 21 | 
 22 | #include "rect.h"
 23 | #include "indexer.h"
 24 | 
 25 | template<class Data, int Dim>
 26 | class DNode : public Data
 27 | {
 28 | public:
 29 |     typedef DNode<Data, Dim> Self;
 30 |     typedef Vector<double, Dim> Vec;
 31 |     typedef Rect<double, Dim> MyRect;
 32 | 
 33 |     int countNodes() const
 34 |     {
 35 |         int nodes = 1;
 36 |         if(children[0] != NULL)
 37 |             for(int i = 0; i < numChildren; ++i)
 38 |             nodes += children[i]->countNodes();
 39 |         return nodes;
 40 |     }
 41 |     
 42 |     int maxLevel() const
 43 |     {
 44 |         if(children[0] == NULL)
 45 |             return 0;
 46 |         int ml = 0;
 47 |         for(int i = 0; i < numChildren; ++i)
 48 |             ml = max(ml, children[i]->maxLevel());
 49 |         return 1 + ml;
 50 |     }
 51 | 
 52 |     Self *getParent() const { return parent; }
 53 |     Self *getChild(int idx) const { return children[idx]; }
 54 |     const MyRect &getRect() const { return rect; }
 55 |     int getChildIndex() const { return childIndex; }
 56 | 
 57 |     static const int numChildren = 1 << Dim;
 58 | 
 59 | private:
 60 |     DNode(MyRect r) : Data(this), parent(NULL), rect(r)
 61 |     {
 62 |         zeroChildren();
 63 |         Data::init();
 64 |     }
 65 | 
 66 |     DNode(Self *inParent, int inChildIndex) : Data(this), parent(inParent), childIndex(inChildIndex)
 67 |     {
 68 |         zeroChildren();
 69 |         rect = MyRect(inParent->rect.getCorner(childIndex)) | MyRect(inParent->rect.getCenter());
 70 |         Data::init();
 71 |     }
 72 | 
 73 |     ~DNode()
 74 |     { 
 75 |         for(int i = 0; i < numChildren; ++i)
 76 |             if(children[i])
 77 |                 delete children[i];
 78 |     }
 79 | 
 80 |     void split()
 81 |     {
 82 |         for(int i = 0; i < numChildren; ++i)
 83 |             children[i] = new Self(this, i);
 84 |     }
 85 | 
 86 |     template<class D, int DI, template<typename N, int ID> class IDX> friend class DRootNode;
 87 | 
 88 |     void zeroChildren() { for(int i = 0; i < numChildren; ++i) children[i] = NULL; }
 89 | 
 90 |     //data
 91 |     Self *parent;
 92 |     Self *children[numChildren];
 93 |     int childIndex;
 94 | 
 95 |     MyRect rect;
 96 | };
 97 | 
 98 | template<class Data, int Dim, template<typename Node, int IDim> class Indexer = DumbIndexer>
 99 | class DRootNode : public DNode<Data, Dim>, public Indexer<DNode<Data, Dim>, Dim>
100 | {
101 | public:
102 |     typedef DNode<Data, Dim> Node;
103 |     typedef DRootNode<Data, Dim, Indexer> Self;
104 |     typedef Indexer<Node, Dim> MyIndexer;
105 |     typedef Vector<double, Dim> Vec;
106 |     typedef Rect<double, Dim> MyRect;
107 | 
108 |     DRootNode(MyRect r = MyRect(Vec(), Vec().apply(bind2nd(plus<double>(), 1.)))) : Node(r)
109 |     {
110 |         MyIndexer::setRoot(this);
111 |     }
112 |     
113 |     void split(Node *node)
114 |     {
115 |         node->split();
116 |     }
117 | };
118 | 
119 | #endif
120 | 


--------------------------------------------------------------------------------
/Pinocchio/pinocchioApi.h:
--------------------------------------------------------------------------------
 1 | /*  This file is part of the Pinocchio automatic rigging library.
 2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
 3 | 
 4 |     This library is free software; you can redistribute it and/or
 5 |     modify it under the terms of the GNU Lesser General Public
 6 |     License as published by the Free Software Foundation; either
 7 |     version 2.1 of the License, or (at your option) any later version.
 8 | 
 9 |     This library is distributed in the hope that it will be useful,
10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12 |     Lesser General Public License for more details.
13 | 
14 |     You should have received a copy of the GNU Lesser General Public
15 |     License along with this library; if not, write to the Free Software
16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
17 | */
18 | 
19 | #ifndef PINOCCHIOAPI_H
20 | #define PINOCCHIOAPI_H
21 | 
22 | #include "mesh.h"
23 | #include "quaddisttree.h"
24 | #include "attachment.h"
25 | 
26 | struct PinocchioOutput
27 | {
28 |     PinocchioOutput() : attachment(NULL) {}
29 | 
30 |     vector<Vector3> embedding;
31 |     Attachment *attachment; //user responsible for deletion
32 | };
33 | 
34 | //calls the other functions and does the whole rigging process
35 | //see the implementation of this function to find out how to use the individual functions
36 | PinocchioOutput PINOCCHIO_API autorig(const Skeleton &given, const Mesh &m);
37 | 
38 | //============================================individual steps=====================================
39 | 
40 | //fits mesh inside unit cube, makes sure there's exactly one connected component
41 | Mesh PINOCCHIO_API prepareMesh(const Mesh &m);
42 | 
43 | 
44 | typedef DRootNode<DistData<3>, 3, ArrayIndexer> TreeType; //our distance field octree type
45 | static const double defaultTreeTol = 0.003;
46 | 
47 | //constructs a distance field on an octree--user responsible for deleting output
48 | TreeType PINOCCHIO_API *constructDistanceField(const Mesh &m, double tol = defaultTreeTol);
49 | 
50 | struct Sphere {
51 |     Sphere() : radius(0.) {}
52 |     Sphere(const Vector3 &inC, double inR) : center(inC), radius(inR) {}
53 | 
54 |     Vector3 center;
55 |     double radius;
56 | };
57 | 
58 | //samples the distance field to find spheres on the medial surface
59 | //output is sorted by radius in decreasing order
60 | vector<Sphere> PINOCCHIO_API sampleMedialSurface(TreeType *distanceField, double tol = defaultTreeTol);
61 | 
62 | //takes sorted medial surface samples and sparsifies the vector
63 | vector<Sphere> PINOCCHIO_API packSpheres(const vector<Sphere> &samples, int maxSpheres = 1000);
64 | 
65 | //constructs graph on packed sphere centers
66 | PtGraph PINOCCHIO_API connectSamples(TreeType *distanceField, const vector<Sphere> &spheres);
67 | 
68 | //finds which joints can be embedded into which sphere centers
69 | vector<vector<int> > PINOCCHIO_API computePossibilities(const PtGraph &graph, const vector<Sphere> &spheres,
70 |                                                         const Skeleton &skeleton);
71 | 
72 | //finds discrete embedding
73 | vector<int> PINOCCHIO_API discreteEmbed(const PtGraph &graph, const vector<Sphere> &spheres,
74 |                                         const Skeleton &skeleton, const vector<vector<int> > &possibilities);
75 | 
76 | //reinserts joints for unreduced skeleton
77 | vector<Vector3> PINOCCHIO_API splitPaths(const vector<int> &discreteEmbedding, const PtGraph &graph,
78 |                                          const Skeleton &skeleton);
79 | 
80 | //refines embedding
81 | vector<Vector3> PINOCCHIO_API refineEmbedding(TreeType *distanceField, const vector<Vector3> &medialSurface,
82 |                                               const vector<Vector3> &initialEmbedding, const Skeleton &skeleton);
83 | 
84 | //to compute the attachment, create a new Attachment object
85 | 
86 | #endif //PINOCCHIOAPI_H
87 | 


--------------------------------------------------------------------------------
/Pinocchio/skeleton.h:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #ifndef SKELETON_H
 20 | #define SKELETON_H
 21 | 
 22 | #include <map>
 23 | #include "graphutils.h"
 24 | 
 25 | class PINOCCHIO_API Skeleton {
 26 | public:
 27 |     Skeleton() {}
 28 |     
 29 |     const PtGraph &fGraph() const { return fGraphV; }
 30 |     const vector<int> &fPrev() const { return fPrevV; }
 31 |     const vector<int> &fSym() const { return fSymV; }
 32 |     
 33 |     const PtGraph &cGraph() const { return cGraphV; }
 34 |     const vector<int> &cPrev() const { return cPrevV; }
 35 |     const vector<int> &cSym() const { return cSymV; }
 36 |     const vector<bool> &cFeet() const { return cFeetV; }
 37 |     const vector<bool> &cFat() const { return cFatV; }
 38 |     
 39 |     const vector<int> &cfMap() const { return cfMapV; }
 40 |     const vector<int> &fcMap() const { return fcMapV; }
 41 |     const vector<double> &fcFraction() const { return fcFractionV; }
 42 |     const vector<double> &cLength() const { return cLengthV; }
 43 | 
 44 |     int getJointForName(const std::string &name) const { if(jointNames.count(name)) return jointNames.find(name)->second; return -1; }
 45 |     
 46 |     void scale(double factor);
 47 |     
 48 | protected:
 49 |     void initCompressed();
 50 |     
 51 |     //help for creation
 52 |     map<string, int> jointNames;
 53 |     void makeJoint(const string &name, const Vector3 &pos, const string &previous = string());
 54 |     void makeSymmetric(const string &name1, const string &name2);
 55 |     void setFoot(const string &name);
 56 |     void setFat(const string &name);
 57 |     
 58 | private:
 59 |      //full
 60 |     PtGraph fGraphV;
 61 |     vector<int> fPrevV; //previous vertices
 62 |     vector<int> fSymV; //symmetry
 63 |     
 64 |     //compressed (no degree 2 vertices)
 65 |     PtGraph cGraphV; 
 66 |     vector<int> cPrevV; //previous vertices
 67 |     vector<int> cSymV; //symmetry
 68 |     vector<bool> cFeetV; //whether the vertex should be near the ground
 69 |     vector<bool> cFatV; //whether the vertex should be in a large region
 70 |     
 71 |     vector<int> cfMapV; //compressed to full map
 72 |     vector<int> fcMapV; //full to compressed map, -1 when vertex is not in compressed
 73 |     vector<double> fcFractionV; //maps full vertex number to ratio of its prev edge length to total length of
 74 |                                 //containing edge in the compressed graph
 75 |     vector<double> cLengthV; //lengths of the compressed bones
 76 | };
 77 | 
 78 | class PINOCCHIO_API HumanSkeleton : public Skeleton
 79 | {
 80 | public:
 81 |     HumanSkeleton();
 82 | };
 83 | 
 84 | class PINOCCHIO_API QuadSkeleton : public Skeleton
 85 | {
 86 | public:
 87 |     QuadSkeleton();
 88 | };
 89 | 
 90 | class PINOCCHIO_API HorseSkeleton : public Skeleton
 91 | {
 92 | public:
 93 |     HorseSkeleton();
 94 | };
 95 | 
 96 | class PINOCCHIO_API CentaurSkeleton : public Skeleton
 97 | {
 98 | public:
 99 |     CentaurSkeleton();
100 | };
101 | 
102 | class PINOCCHIO_API FileSkeleton : public Skeleton
103 | {
104 | public:
105 |     FileSkeleton(const std::string &filename);
106 | }; 
107 | 
108 | #endif
109 | 


--------------------------------------------------------------------------------
/Pinocchio/deriv.h:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #ifndef DERIV_H_INCLUDED
 20 | #define DERIV_H_INCLUDED
 21 | 
 22 | #include "vector.h"
 23 | 
 24 | template<class Real, int Vars>
 25 | class Deriv
 26 | {
 27 | public:
 28 |     typedef Deriv<Real, Vars> Self;
 29 |     
 30 |     Deriv() : x(Real()) {}
 31 |     Deriv(const Real &inX) : x(inX) {}
 32 |     Deriv(const Real &inX, int varNum) : x(inX) { d[varNum] = Real(1.); }
 33 |     Deriv(const Self &inD) : x(inD.x), d(inD.d) {}
 34 |     Deriv(const Real &inX, const Vector<Real, Vars> &inD) : x(inX), d(inD) {}
 35 |     
 36 |     Real getReal() const { return x; }
 37 |     Real getDeriv(int num = 0) const { return d[num]; }
 38 |     
 39 |     Self operator*(const Self &other) const { return Self(x * other.x, x * other.d + other.x * d); }
 40 |     Self operator+(const Self &other) const { return Self(x + other.x, d + other.d); }
 41 |     Self operator-(const Self &other) const { return Self(x - other.x, d - other.d); }
 42 |     Self operator/(const Self &other) const { return Self(x / other.x, (other.x * d - x * other.d) / SQR(other.x)); }
 43 |     Self operator-() const { return Self(-x, -d); }
 44 |     Self &operator+=(const Self &other) { x += other.x; d += other.d; return *this; }
 45 |     Self &operator-=(const Self &other) { x -= other.x; d -= other.d; return *this; }
 46 |     Self &operator*=(const Self &other) { (*this) = (*this) * other; return *this; }
 47 |     Self &operator/=(const Self &other) { (*this) = (*this) / other; return *this; }
 48 | 
 49 |     bool operator<(const Self &other) const { return x < other.x; }
 50 |     bool operator<=(const Self &other) const { return x <= other.x; }
 51 |     bool operator>(const Self &other) const { return x > other.x; }
 52 |     bool operator>=(const Self &other) const { return x >= other.x; }
 53 |     bool operator==(const Self &other) const { return x == other.x; }
 54 |     bool operator!=(const Self &other) const { return x != other.x; }
 55 |     
 56 |     operator Real() const { return x; }
 57 |     
 58 |     //for internal use
 59 |     const Real &_x() const { return x; }
 60 |     const Vector<Real, Vars> &_d() const { return d; }
 61 |         
 62 | private:
 63 |     
 64 |     Real x;
 65 |     Vector<Real, Vars> d;
 66 | };
 67 | 
 68 | #define DerivRV Deriv<Real, Vars>
 69 | #define ONEVAR(func, deriv) template<class Real, int Vars> \
 70 |     DerivRV func(const DerivRV &x) { return DerivRV(func(x._x()), x._d() * (deriv)); }
 71 | 
 72 | ONEVAR(sqrt, Real(0.5) / sqrt(x._x()))
 73 | ONEVAR(log, Real(1.) / x._x())
 74 | ONEVAR(log10, Real(0.43429448190325182765) / x._x()) //the number is 1 / log(10)
 75 | ONEVAR(exp, exp(x._x()))
 76 | ONEVAR(sin, cos(x._x()))
 77 | ONEVAR(cos, -sin(x._x()))
 78 | ONEVAR(tan, Real(1.) / SQR(cos(x._x())))
 79 | ONEVAR(acos, Real(-1.) / sqrt(Real(1. - SQR(x._x()))))
 80 | ONEVAR(asin, Real(1.) / sqrt(Real(1. - SQR(x._x()))))
 81 | ONEVAR(atan, Real(1.) / (Real(1.) + SQR(x._x())))
 82 | ONEVAR(fabs, x._x() < Real(0.) ? Real(-1.) : Real(1.))
 83 | 
 84 | #undef ONEVAR
 85 | #define TWOVAR(func, derivx, derivy) template<class Real, int Vars> \
 86 | DerivRV func(const DerivRV &x, const DerivRV &y) { return DerivRV(func(x._x(), y._x()), x._d() * (derivx) + y._d() * (derivy)); }
 87 | 
 88 | TWOVAR(pow, y._x() * pow(x._x(), y._x() - Real(1.)), log(x._x()) * pow(x._x(), y._x()))
 89 | TWOVAR(atan2, y._x() / (SQR(x._x()) + SQR(y._x())), -x._x() / (SQR(x._x()) + SQR(y._x())))
 90 | 
 91 | #undef TWOVAR
 92 | #undef DerivRV
 93 | 
 94 | template <class charT, class traits, class Real, int Vars>
 95 | basic_ostream<charT,traits>& operator<<(basic_ostream<charT,traits>& os, const Deriv<Real, Vars> &d)
 96 | {
 97 |     os << "Deriv(" << d._x() << ", " << d._d() << ")";
 98 |     return os;
 99 | }
100 | 
101 | #endif //DERIV_H_INCLUDED
102 | 


--------------------------------------------------------------------------------
/Pinocchio/intersector.cpp:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #include "intersector.h"
 20 | 
 21 | #include <queue>
 22 | #include <set>
 23 | 
 24 | //------------------Intersector-----------------
 25 | 
 26 | static const int cells = 200;
 27 | 
 28 | void Intersector::getIndex(const Vector2 &pt, int &x, int &y) const
 29 | {
 30 |     Vector2 c = (pt - bounds.getLo()).apply(divides<double>(), bounds.getSize());
 31 |     x = int(c[0] * double(cells));
 32 |     y = int(c[0] * double(cells));
 33 |     x = max(0, min(cells - 1, x));
 34 |     y = max(0, min(cells - 1, y));
 35 | }
 36 | 
 37 | void Intersector::init()
 38 | {
 39 |     int i, j, k;
 40 |     const vector<MeshVertex> &vtc = mesh->vertices;
 41 |     const vector<MeshEdge> &edg = mesh->edges;
 42 |     
 43 |     dir = dir.normalize();
 44 |     getBasis(dir, v1, v2);
 45 |     
 46 |     points.resize(vtc.size());
 47 |     sNormals.resize(edg.size() / 3);
 48 |     for(i = 0; i < (int)vtc.size(); ++i) {
 49 |         points[i] = Vector2(vtc[i].pos * v1, vtc[i].pos * v2);
 50 |     }
 51 |     
 52 |     bounds = Rect2(points.begin(), points.end());
 53 |     
 54 |     triangles.resize(cells * cells);
 55 |     for(i = 0; i < (int)edg.size(); i += 3) {
 56 |         Rect2 triRect;
 57 |         for(j = 0; j < 3; ++j)
 58 |             triRect |= Rect2(points[edg[i + j].vertex]);
 59 |         
 60 |         int fromx, fromy, tox, toy;
 61 |         getIndex(triRect.getLo(), fromx, fromy);
 62 |         getIndex(triRect.getHi(), tox, toy);
 63 |         
 64 |         for(j = fromy; j <= toy; ++j) for(k = fromx; k <= tox; ++k) {
 65 |             triangles[j * cells + k].push_back(i);
 66 |         }
 67 |         
 68 |         Vector3 cross = (vtc[edg[i + 1].vertex].pos - vtc[edg[i].vertex].pos) % (vtc[edg[i + 2].vertex].pos - vtc[edg[i].vertex].pos);
 69 |         j = i / 3;
 70 |         sNormals[j] = cross.normalize();
 71 |         if(fabs(sNormals[j] * dir) <= 1e-8)
 72 |             sNormals[j] = Vector3(); //zero if coplanar
 73 |         else
 74 |             sNormals[j] = sNormals[j] / (sNormals[j] * dir); //prescaled for intersection
 75 |     }
 76 | }
 77 | 
 78 | vector<Vector3> Intersector::intersect(const Vector3 &pt, vector<int> *outIndices) const
 79 | {
 80 |     int i;
 81 |     const vector<MeshVertex> &vtc = mesh->vertices;
 82 |     const vector<MeshEdge> &edg = mesh->edges;
 83 |     
 84 |     vector<Vector3> out;
 85 |     
 86 |     Vector2 pt2(pt * v1, pt * v2);
 87 |     if(!bounds.contains(pt2))
 88 |         return out; //no intersections
 89 |     
 90 |     int x, y;
 91 |     getIndex(pt2, x, y);
 92 |     const vector<int> &tris = triangles[y * cells + x];
 93 |     for(i = 0; i < (int)tris.size(); ++i) {
 94 |         int j;
 95 |         //check if triangle intersects line
 96 |         int sign[3];
 97 |         int idx[3];
 98 |         for(j = 0; j < 3; ++j) {
 99 |             idx[j] = edg[tris[i] + j].vertex;
100 |         }
101 |         for(j = 0; j < 3; ++j) {
102 |             Vector2 d1 = points[idx[(j + 1) % 3]] - points[idx[j]];
103 |             Vector2 d2 = pt2 - points[idx[j]];
104 |             sign[j] = SIGN(d1[0] * d2[1] - d1[1] * d2[0]);
105 |         }
106 |         if(sign[0] != sign[1] || sign[1] != sign[2])
107 |             continue; //no intersection
108 | 
109 |         if(outIndices)
110 | 	       outIndices->push_back(tris[i]);
111 |         
112 |         //now compute the plane intersection
113 |         const Vector3 &n = sNormals[tris[i] / 3];
114 |         if(n.lengthsq() == 0) { //triangle and line coplanar --just project the triangle center to the line and hope for the best
115 |             Vector3 ctr = (vtc[idx[0]].pos + vtc[idx[1]].pos + vtc[idx[2]].pos) * (1. / 3.);
116 |             out.push_back(projToLine(ctr, pt, dir));
117 |             continue;
118 |         }
119 | 
120 |         out.push_back(pt + dir * (n * (vtc[idx[0]].pos - pt))); //intersection
121 |     }
122 |     
123 |     return out;
124 | }
125 | 


--------------------------------------------------------------------------------
/Pinocchio/matrix.cpp:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #include "matrix.h"
 20 | #include "debugging.h"
 21 | 
 22 | namespace EigPrivate
 23 | {
 24 |     pair<int, double> findMaxAbs(const Vectorn<double> &vec, int maxElem)
 25 |     {
 26 |         pair<int, double> out = make_pair(0, fabs(vec[0]));
 27 |         for(int i = 1; i < maxElem; ++i)
 28 |             if(vec[i] > out.second)
 29 |                 out = make_pair(i, fabs(vec[i]));
 30 |         return out;
 31 |     }
 32 | 
 33 |     void jacobi(int row, int col, Matrixn<double> &m, Matrixn<double> *vectors)
 34 |     {
 35 |         double num = m[row][row] - m[col][col];
 36 |         double dem = 2. * m[row][col];
 37 | 
 38 |         double theta = num / dem;
 39 |         double t;
 40 |         if(fabs(theta) > 1e20) {
 41 |             t = m[row][col] / num;
 42 |         }
 43 |         else {
 44 |             t = (theta > 0 ? 1. : -1.) / (fabs(theta) + sqrt(theta * theta + 1.));
 45 |         }
 46 |         
 47 |         double c = 1. / sqrt(t * t + 1.);
 48 |         double s = t * c;
 49 |         
 50 |         double tau = s / (1. + c);
 51 | 
 52 |         //rotation
 53 |         m[col][col] -= t * m[row][col];
 54 |         m[row][row] += t * m[row][col];
 55 | 
 56 |         int i;
 57 |         for(i = 0; i < col; ++i) {
 58 |             double oldCI = m[col][i];
 59 |             m[col][i] -= s * (m[row][i] + tau * m[col][i]);
 60 |             m[row][i] += s * (oldCI - tau * m[row][i]);
 61 |         }
 62 |         for(i = col + 1; i < row; ++i) {
 63 |             double oldIC = m[i][col];
 64 |             m[i][col] -= s * (m[row][i] + tau * m[i][col]);
 65 |             m[row][i] += s * (oldIC - tau * m[row][i]);
 66 |         }
 67 |         for(i = row + 1; i < m.getRows(); ++i) {
 68 |             double oldIC = m[i][col];
 69 |             m[i][col] -= s * (m[i][row] + tau * m[i][col]);
 70 |             m[i][row] += s * (oldIC - tau * m[i][row]);
 71 |         }
 72 | 
 73 |         if(vectors) {
 74 |             for(i = 0; i < m.getRows(); ++i) {
 75 |                 double oldIC = (*vectors)[i][col];
 76 |                 (*vectors)[i][col] -= s * ((*vectors)[i][row] + tau * (*vectors)[i][col]);
 77 |                 (*vectors)[i][row] += s * (oldIC - tau * (*vectors)[i][row]);
 78 |             }
 79 |         }
 80 | 
 81 |         m[row][col] = 0;
 82 |     }
 83 | }
 84 | 
 85 | using namespace EigPrivate;
 86 | 
 87 | Vectorn<double> getEigensystem(Matrixn<double> m, Matrixn<double> *vectors)
 88 | {
 89 |     int i;
 90 |     assert(m.getRows() == m.getCols());
 91 |     int sz = m.getRows();
 92 |     assert(sz > 1);
 93 |     if(vectors)
 94 |         *vectors = Matrixn<double>::identity(sz);
 95 |     
 96 |     const double tol = 1e-12;
 97 |     int iter = 0;
 98 |     while(++iter < 50) {
 99 |         int j;
100 |         double biggestSoFar = -1;
101 |         for(i = 0; i < sz; ++i) for(j = 0; j < i; ++j) {
102 |             biggestSoFar = max(biggestSoFar, fabs(m[i][j]));
103 |             jacobi(i, j, m, vectors);
104 |         }
105 |         if(biggestSoFar < tol)
106 |             break;
107 |     }
108 |     Debugging::out() << iter << endl;
109 | 
110 |     Vectorn<double> out(sz);
111 |     for(i = 0; i < sz; ++i)
112 |         out[i] = m[i][i];
113 | 
114 |     //now sort by decreasing eigenvalue
115 |     vector<pair<double, int> > perm;
116 |     for(i = 0; i < sz; ++i) {
117 |         perm.push_back(make_pair(fabs(out[i]), i));
118 |     }
119 |     sort(perm.begin(), perm.end());
120 |     reverse(perm.begin(), perm.end());
121 |     Vectorn<double> oldOut = out;
122 |     
123 |     if(!vectors) {
124 |         for(i = 0; i < sz; ++i)
125 |             out[i] = oldOut[perm[i].second];
126 |     }
127 |     else {
128 |         Matrixn<double> oldVectors = ~(*vectors);
129 |         
130 |         for(i = 0; i < sz; ++i) {
131 |             out[i] = oldOut[perm[i].second];
132 |             (*vectors)[i] = oldVectors[perm[i].second];
133 |         }
134 | 
135 |         *vectors = ~(*vectors);
136 |     }
137 | 
138 |     return out;
139 | }
140 | 
141 | 


--------------------------------------------------------------------------------
/Pinocchio/vecutils.h:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #ifndef VECUTILS_H_INCLUDED
 20 | #define VECUTILS_H_INCLUDED
 21 | 
 22 | #include "vector.h"
 23 | 
 24 | template<class Real>
 25 | void getBasis(const Vector<Real, 3> &n, Vector<Real, 3> &v1, Vector<Real, 3> &v2)
 26 | {
 27 |     if(n.lengthsq() < Real(1e-16)) {
 28 |         v1 = Vector<Real, 3>(1., 0., 0.);
 29 |         v2 = Vector<Real, 3>(0., 1., 0.);
 30 |         return;
 31 |     }
 32 |     if(fabs(n[0]) <= fabs(n[1]) && fabs(n[0]) <= fabs(n[2]))
 33 |         v2 = Vector<Real, 3>(1., 0., 0.);
 34 |     else if(fabs(n[1]) <= fabs(n[2]))
 35 |         v2 = Vector<Real, 3>(0., 1., 0.);
 36 |     else
 37 |         v2 = Vector<Real, 3>(0., 0., 1.);
 38 |     
 39 |     v1 = (n % v2).normalize(); //first basis vector
 40 |     v2 = (n % v1).normalize(); //second basis vector
 41 | }
 42 | 
 43 | template<class Real, int Dim>
 44 | Real distsqToLine(const Vector<Real, Dim> &v, const Vector<Real, Dim> &l, const Vector<Real, Dim> &dir)
 45 | {
 46 |   return max(Real(), (v - l).lengthsq() - SQR((v - l) * dir) / dir.lengthsq());
 47 | }
 48 | 
 49 | template<class Real, int Dim>
 50 | Vector<Real, Dim> projToLine(const Vector<Real, Dim> &v, const Vector<Real, Dim> &l, const Vector<Real, Dim> &dir)
 51 | {
 52 |   return l + (((v - l) * dir) / dir.lengthsq()) * dir;
 53 | }
 54 | 
 55 | template<class Real, int Dim>
 56 | Real distsqToSeg(const Vector<Real, Dim> &v, const Vector<Real, Dim> &p1, const Vector<Real, Dim> &p2)
 57 | {
 58 |   typedef Vector<Real, Dim> Vec;
 59 |   
 60 |   Vec dir = p2 - p1;
 61 |   Vec difp2 = p2 - v;
 62 |   
 63 |   if(difp2 * dir < Real())
 64 |     return difp2.lengthsq();
 65 |   
 66 |   Vec difp1 = v - p1;
 67 |   Real dot = difp1 * dir;
 68 | 
 69 |   if(dot <= Real())
 70 |     return difp1.lengthsq();
 71 | 
 72 |   return max(Real(), difp1.lengthsq() - SQR(dot) / dir.lengthsq());
 73 | }
 74 | 
 75 | template<class Real, int Dim>
 76 | Vector<Real, Dim> projToSeg(const Vector<Real, Dim> &v, const Vector<Real, Dim> &p1, const Vector<Real, Dim> &p2)
 77 | {
 78 |   typedef Vector<Real, Dim> Vec;
 79 | 
 80 |   Vec dir = p2 - p1;
 81 | 
 82 |   if((p2 - v) * dir < Real())
 83 |     return p2;
 84 | 
 85 |   Real dot = (v - p1) * dir;
 86 | 
 87 |   if(dot <= Real())
 88 |     return p1;
 89 | 
 90 |   return p1 + (dot / dir.lengthsq()) * dir;
 91 | }
 92 | 
 93 | //d is distance between centers, r1 and r2 are radii
 94 | template<class Real>
 95 | Real getCircleIntersectionArea(const Real &d, const Real &r1, const Real &r2)
 96 | {
 97 |     Real tol(1e-8);
 98 |     
 99 |     if(r1 + r2 <= d + tol)
100 |         return Real();
101 |     if(r1 + d <= r2 + tol)
102 |         return Real(M_PI) * SQR(r1);
103 |     if(r2 + d <= r1 + tol)
104 |         return Real(M_PI) * SQR(r2);
105 |     
106 |     Real sqrdif = SQR(r1) - SQR(r2);
107 |     Real dsqrdif = SQR(d) - sqrdif; //d^2 - r1^2 + r2^2
108 |     Real a1 = SQR(r1) * acos((SQR(d) + sqrdif) / (Real(2.) * r1 * d));
109 |     Real a2 = SQR(r2) * acos(dsqrdif / (Real(2.) * r2 * d));
110 |     return a1 + a2 - Real(0.5) * sqrt(SQR(Real(2.) * d * r2) - SQR(dsqrdif));
111 | }
112 | 
113 | template<class Real>
114 | Vector<Real, 3> projToTri(const Vector<Real, 3> &from, const Vector<Real, 3> &p1, const Vector<Real, 3> &p2, const Vector<Real, 3> &p3)
115 | {
116 |     typedef Vector<Real, 3> Vec;
117 |     Real tolsq(1e-16);
118 |     
119 |     Vec p2p1 = (p2 - p1);
120 |     Vec p3p1 = (p3 - p1);
121 |     Vec normal = p2p1 % p3p1;
122 | 
123 |     if((p2p1 % (from - p1)) * normal >= Real()) { //inside s1
124 |         bool s2 = ((p3 - p2) % (from - p2)) * normal >= Real();
125 |         bool s3 = (p3p1 % (from - p3)) * normal <= Real();
126 |         
127 |         if(s2 && s3) {
128 |             if(normal.lengthsq() < tolsq)
129 |                 return p1; //incorrect, but whatever...
130 |         
131 |             double dot = (from - p3) * normal;
132 |             return from - (dot / normal.lengthsq()) * normal;
133 |         }
134 |         if(!s3 && (s2 || (from - p3) * p3p1 >= Real()))
135 |             return projToSeg(from, p3, p1);
136 |         return projToSeg(from, p2, p3);
137 |     }
138 |     //outside s1
139 |     if((from - p1) * p2p1 < Real())
140 |         return projToSeg(from, p3, p1);
141 |     if((from - p2) * p2p1 > Real())
142 |         return projToSeg(from, p2, p3);
143 |     return projToLine(from, p1, p2p1);
144 | }
145 | 
146 | #endif //VECUTILS_H_INCLUDED
147 | 


--------------------------------------------------------------------------------
/KinectPuppeteering.ui:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="UTF-8"?>
  2 | <ui version="4.0">
  3 |  <class>Dialog</class>
  4 |  <widget class="QDialog" name="Dialog">
  5 |   <property name="geometry">
  6 |    <rect>
  7 |     <x>0</x>
  8 |     <y>0</y>
  9 |     <width>375</width>
 10 |     <height>306</height>
 11 |    </rect>
 12 |   </property>
 13 |   <property name="windowTitle">
 14 |    <string>Dialog</string>
 15 |   </property>
 16 |   <layout class="QVBoxLayout" name="verticalLayout">
 17 |    <item>
 18 |     <widget class="KinectPuppeteeringViewer" name="widget" native="true">
 19 |      <property name="sizePolicy">
 20 |       <sizepolicy hsizetype="Preferred" vsizetype="Expanding">
 21 |        <horstretch>0</horstretch>
 22 |        <verstretch>0</verstretch>
 23 |       </sizepolicy>
 24 |      </property>
 25 |     </widget>
 26 |    </item>
 27 |    <item>
 28 |     <layout class="QHBoxLayout" name="horizontalLayout">
 29 |      <item>
 30 |       <widget class="QPushButton" name="pushButton_loadmesh">
 31 |        <property name="sizePolicy">
 32 |         <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
 33 |          <horstretch>0</horstretch>
 34 |          <verstretch>0</verstretch>
 35 |         </sizepolicy>
 36 |        </property>
 37 |        <property name="text">
 38 |         <string>Load Mesh</string>
 39 |        </property>
 40 |       </widget>
 41 |      </item>
 42 |      <item>
 43 |       <widget class="QPushButton" name="pushButton_loadskel">
 44 |        <property name="text">
 45 |         <string>Load Skeleton</string>
 46 |        </property>
 47 |       </widget>
 48 |      </item>
 49 |      <item>
 50 |       <widget class="QPushButton" name="pushButton_loadPose">
 51 |        <property name="text">
 52 |         <string>Load Pose</string>
 53 |        </property>
 54 |       </widget>
 55 |      </item>
 56 |     </layout>
 57 |    </item>
 58 |    <item>
 59 |     <layout class="QHBoxLayout" name="horizontalLayout_2">
 60 |      <item>
 61 |       <widget class="QPushButton" name="pushButton_autorig">
 62 |        <property name="enabled">
 63 |         <bool>true</bool>
 64 |        </property>
 65 |        <property name="text">
 66 |         <string>Auto Rig</string>
 67 |        </property>
 68 |       </widget>
 69 |      </item>
 70 |      <item>
 71 |       <widget class="QPushButton" name="pushButton_loadrig">
 72 |        <property name="text">
 73 |         <string>Load Rig</string>
 74 |        </property>
 75 |       </widget>
 76 |      </item>
 77 |      <item>
 78 |       <widget class="QPushButton" name="pushButton_saverig">
 79 |        <property name="text">
 80 |         <string>Save Rig</string>
 81 |        </property>
 82 |       </widget>
 83 |      </item>
 84 |     </layout>
 85 |    </item>
 86 |   </layout>
 87 |  </widget>
 88 |  <customwidgets>
 89 |   <customwidget>
 90 |    <class>KinectPuppeteeringViewer</class>
 91 |    <extends>QWidget</extends>
 92 |    <header>../KinectPuppeteeringViewer.h</header>
 93 |    <container>1</container>
 94 |    <slots>
 95 |     <slot>LoadMesh()</slot>
 96 |     <slot>LoadSkeleton()</slot>
 97 |     <slot>AutoRig()</slot>
 98 |     <slot>LoadPose()</slot>
 99 |     <slot>SaveRig()</slot>
100 |     <slot>LoadRig()</slot>
101 |    </slots>
102 |   </customwidget>
103 |  </customwidgets>
104 |  <resources/>
105 |  <connections>
106 |   <connection>
107 |    <sender>pushButton_autorig</sender>
108 |    <signal>clicked()</signal>
109 |    <receiver>widget</receiver>
110 |    <slot>AutoRig()</slot>
111 |    <hints>
112 |     <hint type="sourcelabel">
113 |      <x>327</x>
114 |      <y>271</y>
115 |     </hint>
116 |     <hint type="destinationlabel">
117 |      <x>199</x>
118 |      <y>128</y>
119 |     </hint>
120 |    </hints>
121 |   </connection>
122 |   <connection>
123 |    <sender>pushButton_loadmesh</sender>
124 |    <signal>clicked()</signal>
125 |    <receiver>widget</receiver>
126 |    <slot>LoadMesh()</slot>
127 |    <hints>
128 |     <hint type="sourcelabel">
129 |      <x>72</x>
130 |      <y>271</y>
131 |     </hint>
132 |     <hint type="destinationlabel">
133 |      <x>199</x>
134 |      <y>128</y>
135 |     </hint>
136 |    </hints>
137 |   </connection>
138 |   <connection>
139 |    <sender>pushButton_loadskel</sender>
140 |    <signal>clicked()</signal>
141 |    <receiver>widget</receiver>
142 |    <slot>LoadSkeleton()</slot>
143 |    <hints>
144 |     <hint type="sourcelabel">
145 |      <x>200</x>
146 |      <y>271</y>
147 |     </hint>
148 |     <hint type="destinationlabel">
149 |      <x>199</x>
150 |      <y>128</y>
151 |     </hint>
152 |    </hints>
153 |   </connection>
154 |   <connection>
155 |    <sender>pushButton_loadPose</sender>
156 |    <signal>clicked()</signal>
157 |    <receiver>widget</receiver>
158 |    <slot>LoadPose()</slot>
159 |    <hints>
160 |     <hint type="sourcelabel">
161 |      <x>435</x>
162 |      <y>271</y>
163 |     </hint>
164 |     <hint type="destinationlabel">
165 |      <x>252</x>
166 |      <y>128</y>
167 |     </hint>
168 |    </hints>
169 |   </connection>
170 |   <connection>
171 |    <sender>pushButton_loadrig</sender>
172 |    <signal>clicked()</signal>
173 |    <receiver>widget</receiver>
174 |    <slot>LoadRig()</slot>
175 |    <hints>
176 |     <hint type="sourcelabel">
177 |      <x>187</x>
178 |      <y>277</y>
179 |     </hint>
180 |     <hint type="destinationlabel">
181 |      <x>187</x>
182 |      <y>110</y>
183 |     </hint>
184 |    </hints>
185 |   </connection>
186 |   <connection>
187 |    <sender>pushButton_saverig</sender>
188 |    <signal>clicked()</signal>
189 |    <receiver>widget</receiver>
190 |    <slot>SaveRig()</slot>
191 |    <hints>
192 |     <hint type="sourcelabel">
193 |      <x>304</x>
194 |      <y>277</y>
195 |     </hint>
196 |     <hint type="destinationlabel">
197 |      <x>187</x>
198 |      <y>110</y>
199 |     </hint>
200 |    </hints>
201 |   </connection>
202 |  </connections>
203 | </ui>
204 | 


--------------------------------------------------------------------------------
/PinocchioUI/defmesh.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright (c) 2007 Ilya Baran
  3 | 
  4 | Permission is hereby granted, free of charge, to any person obtaining a copy
  5 | of this software and associated documentation files (the "Software"), to deal
  6 | in the Software without restriction, including without limitation the rights
  7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  8 | copies of the Software, and to permit persons to whom the Software is
  9 | furnished to do so, subject to the following conditions:
 10 | 
 11 | The above copyright notice and this permission notice shall be included in
 12 | all copies or substantial portions of the Software.
 13 | 
 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 20 | THE SOFTWARE.
 21 | */
 22 | 
 23 | #include "defmesh.h"
 24 | #include "motion.h"
 25 | #include "../Pinocchio/intersector.h"
 26 | 
 27 | vector<Transform<> > DefMesh::computeTransforms() const
 28 | {
 29 |     vector<Transform<> > out;
 30 |     int i;
 31 |     
 32 |     if(motion) {
 33 |         vector<Transform<> > ts;
 34 |         ts = motion->get();
 35 |         
 36 |         double legRatio = getLegRatio();
 37 | 	Vector3 trans = ts[0].getTrans() * legRatio;
 38 | 
 39 |         for(int times = 0; times < 2; ++times) {
 40 | 
 41 | 	  if(times == 1)
 42 | 	    trans += (out[0] * match[0] - out[1] * match[2]);
 43 | 
 44 | 	  out.clear();
 45 | 	  vector<Vector3> tm;
 46 | 	  tm.push_back(match[0]);
 47 | 	  ts[0] = ts[0].linearComponent();
 48 | 	  for(i = 0; i < (int)ts.size(); ++i) {
 49 |             int prevV = origSkel.fPrev()[i + 1];
 50 |             out.push_back(Transform<>(tm[prevV]) * ts[i] * Transform<>(-match[prevV]));
 51 |             tm.push_back(out.back() * match[i + 1]);
 52 | 	  }
 53 | 	  
 54 | 	  for(i = 0; i < (int)out.size(); ++i)
 55 |             out[i] = Transform<>(trans + Vector3(0.5, 0, 0.5)) * out[i];
 56 | 	}
 57 |         
 58 |         return out;
 59 |     }
 60 |     
 61 | 
 62 |     out.push_back(Transform<>(Vector3(0.5, 0, 0.5)));
 63 | 
 64 |     for(i = 1; i < (int)origSkel.fPrev().size(); ++i) {
 65 |         int prevV = origSkel.fPrev()[i];
 66 |         Transform<> cur = out[prevV];
 67 |         cur = cur * Transform<>(match[prevV]) * Transform<>(transforms[i - 1]) * Transform<>(-match[prevV]);
 68 | 
 69 |         out.push_back(cur);
 70 |     }
 71 | 
 72 |     out.erase(out.begin());
 73 |     return out;
 74 | }
 75 | 
 76 | bool reallyDeform = true;
 77 | 
 78 | void DefMesh::updateMesh() const
 79 | {
 80 |     vector<Transform<> > t = computeTransforms();
 81 | 
 82 |     if(motion) {
 83 |         if(footOffsets.empty()) {
 84 |             Intersector s(origMesh, Vector3(0, 1, 0));
 85 |             
 86 |             vector<Vector3> sects;
 87 |             double offs;
 88 | 
 89 |             sects = s.intersect(match[7]);
 90 |             offs = 0;
 91 |             for(int i = 0; i < (int)sects.size(); ++i)
 92 |                 offs = max(offs, match[7][1] - sects[i][1]);
 93 |             const_cast<vector<double> *>(&footOffsets)->push_back(offs);
 94 |             
 95 |             sects = s.intersect(match[11]);
 96 |             offs = 0;
 97 |             for(int i = 0; i < (int)sects.size(); ++i)
 98 |                 offs = max(offs, match[11][1] - sects[i][1]);
 99 |             const_cast<vector<double> *>(&footOffsets)->push_back(offs);
100 |         }
101 | 
102 |         vector<Vector3> pose = motion->getPose();
103 |         vector<Vector3> refPose = motion->getRefPose();
104 |         vector<Vector3> feet;
105 | 
106 |         double legRatio = getLegRatio();
107 |         feet.push_back(pose[15] * legRatio);
108 |         feet.push_back(pose[7] * legRatio);
109 | 
110 |         double widthDiff = 0.3 * ((refPose[8][0] - refPose[4][0]) * legRatio - (match[7][0] - match[11][0]));
111 |         Vector3 offs1 = t[6].getRot() * Vector3(-widthDiff, 0, 0);
112 |         Vector3 offs2 = t[10].getRot() * Vector3(widthDiff, 0, 0);
113 |         offs1[1] = offs2[1] = 0;
114 |         offs1 += /*t[6].getRot() */ Vector3(0, footOffsets[0], 0);
115 |         offs2 += /*t[10].getRot() */ Vector3(0, footOffsets[1], 0);
116 |         feet[0] += offs1;
117 |         feet[1] += offs2;
118 | 
119 |         Vector3 pelvisVec = (refPose[0] - 0.5 * (refPose[4] + refPose[8])) * legRatio;
120 |         Vector3 mpelvisVec = (match[2] - 0.5 * (match[7] + match[11]));
121 |         mpelvisVec += Vector3(0, min(footOffsets[0], footOffsets[1]), 0);
122 |         Vector3 v(0, 1, 0);
123 |         feet.push_back(pose[0] * legRatio + v * (v * (mpelvisVec - pelvisVec)));
124 |         //feet.back()[1] = 0.;
125 | 
126 | #if 0
127 |         Debugging::clear();
128 |         for(int i = 0; i < (int)feet.size(); ++i)
129 |             Debugging::drawCircle(feet[i], 0.01, QPen(Qt::blue));
130 | 
131 |         Debugging::drawLine(feet[0], feet[0] - offs1, QPen(Qt::red));
132 |         Debugging::drawLine(feet[1], feet[1] - offs2, QPen(Qt::red));
133 | #endif
134 | 
135 |         filter.step(t, feet);
136 |         if(reallyDeform)
137 |             curMesh = attachment.deform(origMesh, filter.getTransforms());
138 | 
139 | #if 0
140 |         static int period = 1;
141 |         if(--period == 0) {
142 |             period = 3;
143 |             if(rand() % 40 == 0) 
144 |                 Debugging::clear();
145 |             PtGraph skelGraph = origSkel.fGraph();
146 |             skelGraph.verts = getSkel();
147 |             Debugging::drawGraph(skelGraph, QPen(Qt::red, 5));
148 |         }
149 | #endif
150 |     }
151 |     else
152 |         curMesh = attachment.deform(origMesh, t);
153 | }
154 | 
155 | vector<Vector3> DefMesh::getSkel() const
156 | {
157 |     vector<Vector3> out = match;
158 | 
159 |     vector<Transform<> > t;
160 |     if(motion)
161 |         t = filter.getTransforms();
162 |     else
163 |         t = computeTransforms();
164 |         
165 |     for(int i = 0; i < (int)out.size(); ++i) {
166 |         out[i] = t[max(0, i - 1)] * out[i];
167 |     }
168 |     
169 |     return out;
170 | }
171 | 
172 | double DefMesh::getLegRatio() const
173 | {
174 |     double lengthRatio = fabs(match[7][1] - match[2][1]) / motion->getLegLength();
175 |     double widthRatio = fabs(match[7][0] - match[11][0]) / motion->getLegWidth();
176 | 
177 |     return lengthRatio;
178 |     return max(lengthRatio, min(lengthRatio * 1.4, sqrt(lengthRatio * widthRatio)));
179 | }
180 | 


--------------------------------------------------------------------------------
/Pinocchio/pointprojector.h:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #ifndef POINTPROJECTOR_H
 20 | #define POINTPROJECTOR_H
 21 | 
 22 | #include <set>
 23 | 
 24 | #include "vector.h"
 25 | #include "rect.h"
 26 | #include "vecutils.h"
 27 | #include "debugging.h"
 28 | 
 29 | struct Vec3Object
 30 | {
 31 |     Vec3Object(const Vector3 &inV) : v(inV) {}
 32 |     
 33 |     Rect3 boundingRect() const { return Rect3(v); }
 34 |     double operator[](int i) const { return v[i]; }
 35 |     Vector3 project(const Vector3 &) const { return v; }
 36 |     
 37 |     Vector3 v;
 38 | };
 39 | 
 40 | struct Tri3Object
 41 | {
 42 |     Tri3Object(const Vector3 &inV1, const Vector3 &inV2, const Vector3 &inV3) : v1(inV1), v2(inV2), v3(inV3) {}
 43 |     
 44 |     Rect3 boundingRect() const { return Rect3(v1) | Rect3(v2) | Rect3(v3); }
 45 |     double operator[](int i) const { return v1[i] + v2[i] + v3[i]; } //for comparison only, no need to divide by 3
 46 |     Vector3 project(const Vector3 &v) const { return projToTri(v, v1, v2, v3); }
 47 |     
 48 |     Vector3 v1, v2, v3;
 49 | };
 50 | 
 51 | template<int Dim, class Obj>
 52 | class ObjectProjector
 53 | {
 54 | public:
 55 |     typedef Vector<double, Dim> Vec;
 56 |     typedef Rect<double, Dim> Rec;
 57 | 
 58 |     ObjectProjector() {}
 59 |     ObjectProjector(const vector<Obj> &inObjs) : objs(inObjs)
 60 |     {
 61 |         int i, d;
 62 |         vector<int> orders[Dim];
 63 |     
 64 |         for(d = 0; d < Dim; ++d) {
 65 |             orders[d].reserve(objs.size());
 66 |             for(i = 0; i < (int)objs.size(); ++i)
 67 |                 orders[d].push_back(i);
 68 |             sort(orders[d].begin(), orders[d].end(), DLess(d, objs));
 69 |         }
 70 |     
 71 |         rnodes.reserve((int)objs.size() * 2 - 1);
 72 |         initHelper(orders);
 73 |     }
 74 | 
 75 |     Vec project(const Vec &from) const
 76 |     {
 77 |         double minDistSq = 1e37;
 78 |         Vec closestSoFar;
 79 | 
 80 |         int sz = 1;
 81 |         static pair<double, int> todo[10000];
 82 |         todo[0] = make_pair(rnodes[0].rect.distSqTo(from), 0);
 83 | 
 84 |         while(sz > 0) {
 85 |             if(todo[--sz].first > minDistSq) {
 86 |                 continue;
 87 |             }
 88 |             int cur = todo[sz].second; //the top element that was just popped
 89 |         
 90 |             int c1 = rnodes[cur].child1;
 91 |             int c2 = rnodes[cur].child2;
 92 |         
 93 |             if(c1 >= 0) { //not a leaf
 94 |                 double l1 = rnodes[c1].rect.distSqTo(from);
 95 |                 if(l1 < minDistSq)
 96 |                     todo[sz++] = make_pair(l1, c1);
 97 |             
 98 |                 double l2 = rnodes[c2].rect.distSqTo(from);
 99 |                 if(l2 < minDistSq)
100 |                     todo[sz++] = make_pair(l2, c2);
101 |             
102 |                 if(sz >= 2 && todo[sz - 1].first > todo[sz - 2].first) {
103 |                     swap(todo[sz - 1], todo[sz - 2]);
104 |                 }
105 |                 if(sz > 9995) {//getting close to our array limit
106 |                     Debugging::out() << "Large todo list, likely to fail" << endl;
107 |                 }
108 |                 continue;
109 |             }
110 | 
111 |             //leaf -- consider the object
112 |             Vec curPt = objs[c2].project(from);
113 |             double distSq = (from - curPt).lengthsq();
114 |             if(distSq <= minDistSq) {
115 |                 minDistSq = distSq;
116 |                 closestSoFar = curPt;
117 |             }
118 |         }
119 | 
120 |         return closestSoFar;
121 |     };
122 | 
123 |     struct RNode
124 |     {
125 |         Rec rect;
126 |         int child1, child2; //if child1 is -1, child2 is the object index
127 |     };
128 | 
129 |     const vector<RNode> &getRNodes() const { return rnodes; }
130 | 
131 | private:
132 | 
133 |     struct DL { bool operator()(const pair<double, int> &p1,
134 |                                 const pair<double, int> &p2) const { return p1.first > p2.first; } };
135 |         
136 |     int initHelper(vector<int> orders[Dim], int curDim = 0)
137 |     {
138 |         int out = rnodes.size();
139 |         rnodes.resize(out + 1);
140 |     
141 |         int num = orders[0].size();
142 |         if(num == 1) {
143 |             rnodes[out].rect = objs[orders[0][0]].boundingRect();
144 |             rnodes[out].child1 = -1;
145 |             rnodes[out].child2 = orders[0][0];
146 |         }
147 |         else {
148 |             int i, d;
149 |             vector<int> orders1[Dim], orders2[Dim];
150 |             set<int> left;
151 |             for(i = 0; i < num / 2; ++i)
152 |                 left.insert(orders[curDim][i]);
153 |         
154 |             for(d = 0; d < Dim; ++d) {
155 |                 orders1[d].reserve((num + 1) / 2);
156 |                 orders2[d].reserve((num + 1) / 2);
157 |                 for(i = 0; i < num; ++i) {
158 |                     if(left.count(orders[d][i]))
159 |                         orders1[d].push_back(orders[d][i]);
160 |                     else
161 |                         orders2[d].push_back(orders[d][i]);
162 |                 }
163 |             }
164 |         
165 |             rnodes[out].child1 = initHelper(orders1, (curDim + 1) % Dim);
166 |             rnodes[out].child2 = initHelper(orders2, (curDim + 1) % Dim);
167 |             rnodes[out].rect = rnodes[rnodes[out].child1].rect | rnodes[rnodes[out].child2].rect;
168 |         }
169 |         return out;
170 |     }
171 | 
172 |     class DLess
173 |     {
174 |     public:
175 |         DLess(int inDim, const vector<Obj> &inObjs) : dim(inDim), objs(inObjs) {}
176 |         bool operator()(int i1, int i2) { return objs[i1][dim] < objs[i2][dim]; }
177 |     private:
178 |         int dim;
179 |         const vector<Obj> &objs;
180 |     };
181 | 
182 |     vector<RNode> rnodes;
183 |     vector<Obj> objs;
184 | };
185 | #endif
186 | 


--------------------------------------------------------------------------------
/Pinocchio/rect.h:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #ifndef RECT_H_INCLUDED
 20 | #define RECT_H_INCLUDED
 21 | 
 22 | #include "vector.h"
 23 | 
 24 | namespace _RectPrivate {
 25 | template <int Dim> class RectOp;
 26 | }
 27 | 
 28 | template<class Real, int Dim>
 29 | class Rect {
 30 | public:
 31 |     typedef Vector<Real, Dim> Vec;
 32 |     typedef Rect<Real, Dim> Self;
 33 |     typedef _RectPrivate::RectOp<Dim> RO;
 34 |     
 35 |     Rect() : empty(true) {}
 36 |     Rect(const Vec &vec) : empty(false), lo(vec), hi(vec) {}
 37 |     Rect(const Vec &inLo, const Vec &inHi) : lo(inLo), hi(inHi) { markEmpty(); }
 38 |     Rect(const Rect &inRect) : empty(inRect.empty), lo(inRect.lo), hi(inRect.hi) {}
 39 |     template<class R> Rect(const Rect<R, Dim> &inRect) : empty(inRect.empty), lo(inRect.lo), hi(inRect.hi) {}
 40 |     
 41 |     //constructs a Rect given an iterator over points--could be optimized with a min-max
 42 |     template<class Iter> Rect(Iter start, const Iter &finish) : empty(false)
 43 |     {
 44 |         if(start == finish) {
 45 |             empty = true;
 46 |             return;
 47 |         }
 48 |         lo = hi = *start;
 49 |         while(++start != finish) {
 50 |             lo = lo.apply(minimum<Real>(), *start);
 51 |             hi = hi.apply(maximum<Real>(), *start);
 52 |         }
 53 |     }
 54 |     
 55 |     bool isEmpty() const { return empty; }
 56 |     
 57 |     Self operator&(const Self &other) const
 58 |     {
 59 |         if(!empty && !other.empty)
 60 |             return Self(lo.apply(maximum<Real>(), other.lo), hi.apply(minimum<Real>(), other.hi));
 61 |         return Self();
 62 |     }
 63 |     
 64 |     Self operator|(const Self &other) const
 65 |     {
 66 |         if(empty)
 67 |             return other;
 68 |         if(other.empty)
 69 |             return (*this);
 70 |         return Self(false, lo.apply(minimum<Real>(), other.lo), hi.apply(maximum<Real>(), other.hi));
 71 |     }
 72 |     
 73 |     bool operator==(const Self &other) const
 74 |     {
 75 |         if(empty && other.empty)
 76 |             return true;
 77 |         return (empty == other.empty) && (lo == other.lo) && (hi == other.hi);
 78 |     }
 79 |     
 80 | #define OPAS(op, typ) Self &operator op##=(const typ &x) { (*this) = (*this) op x; return *this; }
 81 |     OPAS(&, Self)
 82 |     OPAS(|, Self)
 83 | #undef OPAS
 84 |     
 85 |     bool contains(const Self &other) const
 86 |     {
 87 |         return other.empty || !(empty || other.lo.accumulate(less<Real>(), logical_or<bool>(), lo) ||
 88 |                 hi.accumulate(less<Real>(), logical_or<bool>(), other.hi));
 89 |     }
 90 |     
 91 |     bool contains(const Vec &other) const
 92 |     {
 93 |         return !(empty || other.accumulate(less<Real>(), logical_or<bool>(), lo) ||
 94 |                 hi.accumulate(less<Real>(), logical_or<bool>(), other));
 95 |     }
 96 |     
 97 |     const Vec &getLo() const { return lo; }
 98 |     const Vec &getHi() const { return hi; }
 99 |     Vec getSize() const { return empty ? Vec() : hi - lo; }
100 |     
101 |     Real getContent() const { return empty ? Real() : (hi - lo).accumulate(ident<Real>(), multiplies<Real>()); }
102 |     Real getDiagLength() const { return empty ? Real() : (hi - lo).length(); }
103 |     Vec getCenter() const { return (lo + hi) / Real(2); }
104 | 
105 |     Real distSqTo(const Vec &other) const { return RO::distSq(*this, other); }
106 |     Real distSqTo(const Self &other) const { return RO::distSq(*this, other); }
107 |     
108 |     Vec getCorner(int idx) const
109 |     {
110 |         Vec out;
111 |         BitComparator<Dim>::assignCorner(idx, hi, lo, out);
112 |         return out;
113 |     }
114 |     
115 | private:
116 |     template<class R, int D> friend class Rect;
117 | 
118 |     Rect(bool inEmpty, const Vec &inLo, const Vec &inHi) : empty(inEmpty), lo(inLo), hi(inHi) { }
119 |     void markEmpty() { empty = hi.accumulate(less<Real>(), logical_or<bool>(), lo); }
120 | 
121 |     bool empty;
122 |     Vec lo, hi;
123 | };
124 | 
125 | typedef Rect<double, 2> Rect2;
126 | typedef Rect<double, 3> Rect3;
127 | 
128 | template <class charT, class traits, class Real, int Dim>
129 |         basic_ostream<charT,traits>& operator<<(basic_ostream<charT,traits>& os, const Rect<Real, Dim> &r)
130 | {
131 |     if(r.isEmpty())
132 |         os << "Rect()";
133 |     else
134 |         os << "Rect(" << r.getLo() << ", " << r.getHi() << ")";
135 |     return os;
136 | }
137 | 
138 | namespace _RectPrivate {
139 | #define VRD Vector<R, D>
140 | #define RRD Rect<R, D>
141 | template <int Dim>
142 | class RectOp
143 | {
144 | private:
145 |     static const int last = Dim - 1;
146 |     typedef RectOp<Dim - 1> Next;
147 |     template<int D> friend class RectOp;
148 |     template<class R, int D> friend class Rect;
149 | 
150 |     template<class R, int D>
151 |     static R distSq(const RRD &r, const VRD &v)
152 |     {
153 |         R out = Next::distSq(r, v);
154 | 	if(r.getLo()[last] > v[last])
155 | 	    return out + SQR(r.getLo()[last] - v[last]);
156 | 	if(r.getHi()[last] < v[last])
157 | 	    return out + SQR(v[last] - r.getHi()[last]);
158 | 	return out;
159 |     }
160 | 
161 |     template<class R, int D>
162 |     static R distSq(const RRD &r, const RRD &r2)
163 |     {
164 |         R out = Next::distSq(r, r2);
165 | 	if(r.getLo()[last] > r2.getHi()[last])
166 | 	    return out + SQR(r.getLo()[last] - r2.getHi()[last]);
167 | 	if(r.getHi()[last] < r2.getLo()[last])
168 | 	    return out + SQR(r2.getLo()[last] - r.getHi()[last]);
169 | 	return out;
170 |     }
171 | 
172 | };
173 | 
174 | template <>
175 | class RectOp<1>
176 | {
177 | private:
178 |     template<int D> friend class RectOp;
179 | 
180 |     template<class R, int D>
181 |     static R distSq(const RRD &r, const VRD &v)
182 |     {
183 | 	if(r.getLo()[0] > v[0])
184 | 	    return SQR(r.getLo()[0] - v[0]);
185 | 	if(r.getHi()[0] < v[0])
186 | 	    return SQR(v[0] - r.getHi()[0]);
187 | 	return R();
188 |     }
189 | 
190 |     template<class R, int D>
191 |     static R distSq(const RRD &r, const RRD &r2)
192 |     {
193 | 	if(r.getLo()[0] > r2.getHi()[0])
194 | 	    return SQR(r.getLo()[0] - r2.getHi()[0]);
195 | 	if(r.getHi()[0] < r2.getLo()[0])
196 | 	    return SQR(r2.getLo()[0] - r.getHi()[0]);
197 | 	return R();
198 |     }
199 | 
200 | };
201 | } //namespace _RectPrivate
202 | 
203 | #endif //RECT_H_INCLUDED
204 | 


--------------------------------------------------------------------------------
/Pinocchio/indexer.h:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #ifndef INDEXER_H
 20 | #define INDEXER_H
 21 | 
 22 | #include "hashutils.h"
 23 | #include "vector.h"
 24 | 
 25 | template<class Node, int Dim>
 26 | class DumbIndexer
 27 | { 
 28 |     public:
 29 |         typedef typename Node::Vec Vec;
 30 |     
 31 |         DumbIndexer() : root(NULL) {}
 32 |     
 33 |         void setRoot(Node *n) 
 34 |         {
 35 |             root = n;
 36 |         } 
 37 |         
 38 |         void preprocessIndex() {}
 39 |     
 40 |         Node *locate(const Vec &v) const
 41 |         {
 42 |             Node *out = root;
 43 |             while(out->getChild(0)) {
 44 |                 Vec center = out->getChild(0)->getRect().getHi();
 45 |                 out = out->getChild(BitComparator<Dim>::less(center, v));
 46 |             }
 47 |             return out;
 48 |         }
 49 |     
 50 |     private:
 51 |         Node *root;
 52 | };
 53 | 
 54 | extern PINOCCHIO_API unsigned int interLeaveLookupTable[32768];
 55 | extern PINOCCHIO_API unsigned int interLeave3LookupTable[1024];
 56 | 
 57 | inline unsigned int _lookup(const Vector2 &vec)
 58 | {
 59 |     return interLeaveLookupTable[int(vec[0] * 32767.999)] + (interLeaveLookupTable[int(vec[1] * 32767.999)] << 1);
 60 | }
 61 | 
 62 | inline unsigned int _lookup(const Vector3 &vec)
 63 | {
 64 |     return interLeave3LookupTable[int(vec[0] * 1023.999)] +
 65 |           (interLeave3LookupTable[int(vec[1] * 1023.999)] << 1) +
 66 |           (interLeave3LookupTable[int(vec[2] * 1023.999)] << 2);
 67 | }
 68 | 
 69 | template<class Node, int Dim>
 70 | class Indexer
 71 | { 
 72 | public:
 73 |     typedef typename Node::Vec Vec;
 74 | 
 75 |     Indexer() : root(NULL) {}
 76 | 
 77 |     void setRoot(Node *n) 
 78 |     {
 79 |         root = n;
 80 |     } 
 81 |     
 82 |     void preprocessIndex() {}
 83 | 
 84 |     Node *locate(const Vec &v) const
 85 |     {
 86 |         Node *out = root;
 87 |         unsigned int idx = _lookup(v);
 88 |         static const int mask = (1 << Dim) - 1;
 89 |         while(out->getChild(idx & mask)) {
 90 |             out = out->getChild(idx & mask);
 91 |             idx = idx >> Dim;
 92 |         }
 93 |         return out;
 94 |     }
 95 | private:
 96 |     Node *root;
 97 | };
 98 | 
 99 | template<class Node, int Dim>
100 | class ArrayIndexer
101 | { 
102 | public:
103 |     typedef typename Node::Vec Vec;
104 | 
105 |     ArrayIndexer() : root(NULL) {}
106 | 
107 |     void setRoot(Node *n) 
108 |     {
109 |         root = n;
110 |     } 
111 | 
112 |     static const int bits = 16 - (16 % Dim);
113 | 
114 |     void preprocessIndex()
115 |     {
116 |         for(int i = 0; i < (1 << bits); ++i) {
117 |             table[i] = root;
118 |             int cur = i;
119 |             int cnt = 0;
120 |             static const int mask = (1 << Dim) - 1;
121 |             while(table[i]->getChild(0) && cnt < (bits / Dim)) {
122 |                 ++cnt;
123 |                 table[i] = table[i]->getChild(cur & mask);
124 |                 cur = cur >> Dim;
125 |             }
126 |         }
127 |     }
128 | 
129 |     Node *locate(const Vec &v) const
130 |     {
131 |         unsigned int idx = _lookup(v);
132 |         Node *out = table[idx & ((1 << bits) - 1)];
133 |         if(!out->getChild(0))
134 |             return out;
135 |         idx = idx >> bits;
136 |         static const int mask = (1 << Dim) - 1;
137 |         do {
138 |             out = out->getChild(idx & mask);
139 |             idx = idx >> Dim;
140 |         } while(out->getChild(idx & mask));
141 |         return out;
142 |     }
143 | private:
144 |     Node *root;
145 |     Node *table[(1 << bits)];
146 | };
147 | 
148 | template<class Node, int Level> class HashIndex
149 | {
150 | public:
151 |     HashIndex() { for(int i = 0; i < num; ++i) nodeMap[i] = make_pair(-1, (Node *)NULL); }
152 | 
153 |     void add(Node *node, unsigned int idx)
154 |     {
155 |         int idxx = (idx >> (Level - bits));
156 |         //int idxx = idx % num;
157 |         if(nodeMap[idxx].first == -1)
158 |             nodeMap[idxx] = make_pair(idx, node);
159 |     }
160 | 
161 |     Node *lookup(unsigned int idx) const
162 |     {
163 |         const pair<int, Node *> &p = nodeMap[(idx >> (Level - bits))];
164 |         //const pair<int, Node *> &p = nodeMap[idx % num];
165 |         return p.first == idx ? p.second : NULL;
166 |     }
167 | 
168 |     static const int bits = 16;
169 |     static const int num = (1 << bits);
170 |     //static const int num = 75437;
171 | 
172 | private:
173 |     pair<int, Node *> nodeMap[num];
174 | };
175 | 
176 | template<class Node, int Dim>
177 | class HashIndexer
178 | { 
179 |     public:
180 |         typedef typename Node::Vec Vec;
181 | 
182 |         HashIndexer() : root(NULL) {}
183 | 
184 |         void setRoot(Node *n) 
185 |         {
186 |             root = n;
187 |         } 
188 | 
189 |         static const int bits = 16;
190 |         static const int hlev = 22;
191 | 
192 |         void preprocessIndex()
193 |         {
194 |             for(int i = 0; i < (1 << bits); ++i) {
195 |                 table[i] = root;
196 |                 int cur = i;
197 |                 int cnt = 0;
198 |                 while(table[i]->getChild(0) && cnt < (bits / 2)) {
199 |                     ++cnt;
200 |                     table[i] = table[i]->getChild(cur & 3);
201 |                     cur = cur >> 2;
202 |                 }
203 |             }
204 |             add(root, 0, 0);
205 |         }
206 | 
207 |         Node *locate(const Vec &v) const
208 |         {
209 |             unsigned int idx = _lookup(v);
210 |             Node *out = table[idx & ((1 << bits) - 1)];
211 |             if(!out->getChild(0))
212 |                 return out;
213 |             Node *n = hNodes.lookup(idx & ((1 << hlev) - 1));
214 |             if(!n)
215 |                 idx = idx >> bits;
216 |             else {
217 |                 out = n;
218 |                 if(!out->getChild(0))
219 |                     return out;
220 |                 idx = idx >> hlev;
221 |             }
222 |             do {
223 |                 out = out->getChild(idx & 3);
224 |                 idx = idx >> 2;
225 |             } while(out->getChild(idx & 3));
226 |             return out;
227 |         }
228 |     private:
229 |         unsigned int getIndex(Node *n) const
230 |         {
231 |             if(n == root)
232 |                 return 0;
233 |             int level = getLevel(n);
234 |             return (n->getChildIndex() << (2 * level - 2)) + getIndex(n->getParent());
235 |         }
236 |         int getLevel(Node *n) const
237 |         {
238 |             if(n == root)
239 |                 return 0;
240 |             return 1 + getLevel(n->getParent());
241 |         }
242 |         
243 |         void add(Node *cur, int level, unsigned int idx)
244 |         {
245 |             if(level == hlev) {
246 |                 hNodes.add(cur, idx);
247 |                 return;
248 |             }
249 |             if(cur->getChild(0) == NULL)
250 |                 return;
251 |             for(int i = 0; i < 4; ++i)
252 |                 add(cur->getChild(i), level + 2, idx + (i << level));
253 |         }
254 | 
255 |         Node *root;
256 |         Node *table[(1 << bits)];
257 |         HashIndex<Node, hlev> hNodes;
258 | };
259 | #endif
260 | 


--------------------------------------------------------------------------------
/PinocchioUI/filter.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright (c) 2007 Ilya Baran
  3 | 
  4 | Permission is hereby granted, free of charge, to any person obtaining a copy
  5 | of this software and associated documentation files (the "Software"), to deal
  6 | in the Software without restriction, including without limitation the rights
  7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  8 | copies of the Software, and to permit persons to whom the Software is
  9 | furnished to do so, subject to the following conditions:
 10 | 
 11 | The above copyright notice and this permission notice shall be included in
 12 | all copies or substantial portions of the Software.
 13 | 
 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 20 | THE SOFTWARE.
 21 | */
 22 | 
 23 | #include "filter.h"
 24 | #include "../Pinocchio/deriv.h"
 25 | 
 26 | template<class Real>
 27 | Vectorn<Real> getFeet(const vector<Transform<Real> > &transforms, const vector<Vector3> &joints,
 28 |                                   const vector<int> &prev)
 29 | {
 30 |     int i;
 31 |     Vectorn<Real> out;
 32 |     Vectorn<Vector<Real, 3> > results(joints.size());
 33 | 
 34 |     results[0] = transforms[0] * Vector<Real, 3>(joints[0]);
 35 |     for(i = 1; i < (int)joints.size(); ++i) {
 36 |         results[i] = results[prev[i]] + transforms[i - 1].getRot() * Vector<Real, 3>(joints[i] - joints[prev[i]]);
 37 |     }
 38 | 
 39 |     out.push_back(results[7][0]);
 40 |     out.push_back(results[7][1]);
 41 |     out.push_back(results[7][2]);
 42 |     out.push_back(results[11][0]);
 43 |     out.push_back(results[11][1]);
 44 |     out.push_back(results[11][2]);
 45 |     out.push_back(results[2][0]);
 46 |     out.push_back(results[2][1]);
 47 |     out.push_back(results[2][2]);
 48 | 
 49 |     return out;
 50 | }
 51 | 
 52 | Vectorn<double> toVector(const vector<Transform<> > &transforms)
 53 | {
 54 |     Vectorn<double> out(3 + 4 * transforms.size());
 55 |     out[0] = transforms[0].getTrans()[0];
 56 |     out[1] = transforms[0].getTrans()[1];
 57 |     out[2] = transforms[0].getTrans()[2];
 58 | 
 59 |     for(int i = 0; i < (int)transforms.size(); ++i) {
 60 |         out[3 + i * 4 + 0] = transforms[i].getRot()[0];
 61 |         out[3 + i * 4 + 1] = transforms[i].getRot()[1];
 62 |         out[3 + i * 4 + 2] = transforms[i].getRot()[2];
 63 |         out[3 + i * 4 + 3] = transforms[i].getRot()[3];
 64 |     }
 65 | 
 66 |     return out;
 67 | }
 68 | 
 69 | vector<Transform<> > fromVector(const Vectorn<double> &v)
 70 | {
 71 |     vector<Transform<> > out;
 72 | 
 73 |     int i;
 74 |     Vector3 trans0(v[0], v[1], v[2]);
 75 |     for(i = 3; i + 3 < (int)v.size(); i += 4) {
 76 |         Quaternion<> rot;
 77 |         rot.set(v[i], Vector3(v[i + 1], v[i + 2], v[i + 3]));
 78 | 
 79 |         if(i == 3)
 80 |             out.push_back(Transform<>(rot, 1., trans0));
 81 |         else
 82 |             out.push_back(Transform<>(rot));
 83 |     }
 84 | 
 85 |     return out;
 86 | }
 87 | 
 88 | Vectorn<double> adjVector(const Vectorn<double> &v, const Vectorn<double> &dirs)
 89 | {
 90 |     Vectorn<double> out = v;
 91 |     int i;
 92 |     for(i = 3; i + 3 < (int)v.size(); i += 4) {
 93 |         if(Vectorn<double>(v.begin() + i, v.begin() + i + 4) *
 94 |            Vectorn<double>(dirs.begin() + i, dirs.begin() + i + 4) < 0.) {
 95 |             for(int j = i; j < i + 4; ++j)
 96 |                 out[j] = -out[j];
 97 |         }
 98 |     }
 99 |     return out;
100 | }
101 | 
102 | Matrixn<double> MotionFilter::getJac(const vector<Transform<> > &transforms) const
103 | {
104 |     typedef Deriv<double, -1> D;
105 |     vector<Transform<D> > transD(transforms.size());
106 |     int i, j;
107 | 
108 |     Vectorn<double> transVec = toVector(transforms);
109 |     Vector<D, 3> trans0;
110 |     for(i = 0; i < 3; ++i)
111 |         trans0[i] = D(transVec[i], i);
112 | 
113 |     for(i = 0; i < (int)transforms.size(); ++i) {
114 |         int curIdx = 3 + i * 4;
115 | 
116 |         Quaternion<D> rot;
117 |         rot.set(D(transVec[curIdx], curIdx), Vector<D, 3>(D(transVec[curIdx + 1], curIdx + 1),
118 |                                                           D(transVec[curIdx + 2], curIdx + 2),
119 |                                                           D(transVec[curIdx + 3], curIdx + 3)));
120 |         
121 |         if(i == 0)
122 |             transD[i] = Transform<D>(rot, 1., trans0);
123 |         else
124 |             transD[i] = Transform<D>(rot);
125 |     }
126 | 
127 |     Vectorn<D> feet = getFeet(transD, joints, prev);
128 |     Matrixn<double> out(feet.size(), 3 + 4 * transforms.size());
129 |     for(i = 0; i < out.getRows(); ++i) for(j = 0; j < out.getCols(); ++j) {
130 |         out[i][j] = feet[i].getDeriv(j);
131 |     }
132 | 
133 |     return out;
134 | }
135 | 
136 | void MotionFilter::step(const vector<Transform<> > &transforms, vector<Vector3> feet)
137 | {
138 |     Vectorn<double> feetV;
139 |     int i, j;
140 |     for(i = 0; i < (int)feet.size(); ++i) for(j = 0; j < 3; ++j)
141 |         feetV.push_back(feet[i][j]);
142 | 
143 |     if(curTransforms.size() == 0) {
144 |         prevFeet = feetV;
145 |         prevTrans = feet.back();
146 |         curTransforms = transforms;
147 |         addTranslation();
148 |         return;
149 |     }
150 | 
151 |     if(false) {
152 |         curTransforms = fromVector(toVector(transforms));
153 |         addTranslation();
154 |         return;
155 |     }
156 | 
157 |     //flips quaternions on incoming transforms to be more like current ones
158 |     vector<Transform<> > transfs = fromVector(adjVector(toVector(transforms), toVector(curTransforms)));
159 | 
160 |     Matrixn<double> jac = getJac(curTransforms);
161 | 
162 | #if 1 //regular pseudoinverse
163 |     Matrixn<double> regularizer = Matrixn<double>::identity(jac.getRows(), 1e-4);
164 |     regularizer[6][6] = regularizer[7][7] = regularizer[8][8] = 1e-2;
165 |     Matrixn<double> jacPI = ~jac * !(jac * ~jac + regularizer);
166 | #else //svd based pseudoinverse
167 |     Matrixn<double> u, v;
168 |     Vectorn<double> sigma;
169 |     
170 |     sigma = getSVD(~jac, u, v);
171 |     double minSV = 1e-6;
172 |     for(i = 0; i < (int)sigma.size(); ++i) {
173 |         if(fabs(sigma[i]) < minSV) { //?
174 |             sigma[i] = sigma[i] > 0 ? minSV : -minSV;
175 |         }
176 |         sigma[i] = 1. / sigma[i];
177 |     }
178 |     
179 |     Matrixn<double> jacPI = u * (Matrixn<double>::identity(sigma) * ~v);
180 | #endif
181 | 
182 |     Vectorn<double> delta = feetV - prevFeet;
183 |     if(delta.length() > .1)
184 |         delta = delta.normalize() * .1;
185 | 
186 |     Vectorn<double> error = (feetV - getFeet(curTransforms, joints, prev)) * 0.5;
187 | 
188 |     delta = delta + error;
189 | 
190 |     Vectorn<double> tDelta = jacPI * delta;
191 | 
192 |     Matrixn<double> projMatrix = Matrixn<double>::identity(jac.getCols()) - jacPI * jac;
193 | 
194 |     Vectorn<double> transVector = toVector(transfs) - toVector(curTransforms);
195 |     Vector3 newTrans = 0. * (transfs[0].getTrans() - prevTrans /*- Vector3(tDelta[0], tDelta[1], tDelta[2])*/);
196 |     transVector[0] = newTrans[0];
197 |     transVector[1] = newTrans[1];
198 |     transVector[2] = newTrans[2];
199 |     tDelta = tDelta + projMatrix * transVector * 0.8;
200 |     
201 |     curTransforms = fromVector(toVector(curTransforms) + tDelta);
202 |     addTranslation();
203 | 
204 |     prevFeet = feetV;
205 |     prevTrans = transfs[0].getTrans();
206 | }
207 | 
208 | void MotionFilter::addTranslation()
209 | {
210 |     int i;
211 |     vector<Vector3> pts = joints;
212 |     
213 |     pts[0] = curTransforms[0] * pts[0];
214 |     pts[1] = curTransforms[0] * pts[1];
215 |     for(i = 1; i < (int)curTransforms.size(); ++i) {
216 |         int pi = prev[i + 1];
217 | 
218 |         curTransforms[i] = Transform<>(pts[pi]) * Transform<>(curTransforms[i].getRot()) * Transform<>(-joints[pi]);
219 | 
220 |         pts[i + 1] = curTransforms[i] * joints[i + 1];
221 |     }
222 | }
223 | 
224 | 


--------------------------------------------------------------------------------
/PinocchioUI/processor.cpp.no:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright (c) 2007 Ilya Baran
  3 | 
  4 | Permission is hereby granted, free of charge, to any person obtaining a copy
  5 | of this software and associated documentation files (the "Software"), to deal
  6 | in the Software without restriction, including without limitation the rights
  7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  8 | copies of the Software, and to permit persons to whom the Software is
  9 | furnished to do so, subject to the following conditions:
 10 | 
 11 | The above copyright notice and this permission notice shall be included in
 12 | all copies or substantial portions of the Software.
 13 | 
 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 20 | THE SOFTWARE.
 21 | */
 22 | 
 23 | #include <FL/fl_ask.H>
 24 | #include <fstream>
 25 | 
 26 | #include "processor.h"
 27 | #include "../Pinocchio/skeleton.h"
 28 | #include "../Pinocchio/utils.h"
 29 | #include "../Pinocchio/debugging.h"
 30 | #include "../Pinocchio/attachment.h"
 31 | #include "../Pinocchio/pinocchioApi.h"
 32 | #include "defmesh.h"
 33 | #include "motion.h"
 34 | 
 35 | struct ArgData
 36 | {
 37 |     ArgData() :
 38 |         stopAtMesh(false), stopAfterCircles(false), skelScale(1.), noFit(false),
 39 |         skeleton(HumanSkeleton())
 40 |     {
 41 |     }
 42 |     
 43 |     bool stopAtMesh;
 44 |     bool stopAfterCircles;
 45 |     string filename;
 46 |     string motionname;
 47 |     Quaternion<> meshTransform;
 48 |     double skelScale;
 49 |     bool noFit;
 50 |     Skeleton skeleton;
 51 |     string skeletonname;
 52 | };
 53 | 
 54 | void printUsageAndExit()
 55 | {
 56 |     cout << "Usage: DemoUI filename.{obj | ply | off | gts | stl}" << endl;
 57 |     cout << "              [-skel skelname] [-rot x y z deg]* [-scale s]" << endl;
 58 |     cout << "              [-meshonly | -mo] [-circlesonly | -co]" << endl;
 59 |     cout << "              [-motion motionname] [-nofit]" << endl;
 60 | 
 61 |     exit(0);
 62 | }
 63 | 
 64 | ArgData processArgs(const vector<string> &args)
 65 | {
 66 |     ArgData out;
 67 |     int cur = 2;
 68 |     int num = args.size();
 69 |     if(num < 2)
 70 |         printUsageAndExit();
 71 | 
 72 |     out.filename = args[1];
 73 |     
 74 |     while(cur < num) {
 75 |         string curStr = args[cur++];
 76 |         if(curStr == string("-skel")) {
 77 |             if(cur == num) {
 78 |                 cout << "No skeleton specified; ignoring." << endl;
 79 |                 continue;
 80 |             }
 81 |             curStr = args[cur++];
 82 |             if(curStr == string("human"))
 83 |                 out.skeleton = HumanSkeleton();
 84 |             else if(curStr == string("horse"))
 85 |                 out.skeleton = HorseSkeleton();
 86 |             else if(curStr == string("quad"))
 87 |                 out.skeleton = QuadSkeleton();
 88 |             else if(curStr == string("centaur"))
 89 |                 out.skeleton = CentaurSkeleton();
 90 |             else
 91 |                 out.skeleton = FileSkeleton(curStr);
 92 |             out.skeletonname = curStr;
 93 |             continue;
 94 |         }
 95 |         if(curStr == string("-rot")) {
 96 |             if(cur + 3 >= num) {
 97 |                 cout << "Too few rotation arguments; exiting." << endl;
 98 |                 printUsageAndExit();
 99 |             }
100 |             double x, y, z, deg;
101 |             sscanf(args[cur++].c_str(), "%lf", &x);
102 |             sscanf(args[cur++].c_str(), "%lf", &y);
103 |             sscanf(args[cur++].c_str(), "%lf", &z);
104 |             sscanf(args[cur++].c_str(), "%lf", &deg);
105 |             
106 |             out.meshTransform = Quaternion<>(Vector3(x, y, z), deg * M_PI / 180.) * out.meshTransform;
107 |             continue;
108 |         }
109 |         if(curStr == string("-scale")) {
110 |             if(cur >= num) {
111 |                 cout << "No scale provided; exiting." << endl;
112 |                 printUsageAndExit();
113 |             }
114 |             sscanf(args[cur++].c_str(), "%lf", &out.skelScale);
115 |             continue;
116 |         }
117 |         if(curStr == string("-meshonly") || curStr == string("-mo")) {
118 |             out.stopAtMesh = true;
119 |             continue;
120 |         }
121 |         if(curStr == string("-circlesonly") || curStr == string("-co")) {
122 |             out.stopAfterCircles = true;
123 |             continue;
124 |         }
125 |         if(curStr == string("-nofit")) {
126 |             out.noFit = true;
127 |             continue;
128 |         }
129 |         if(curStr == string("-motion")) {
130 |             if(cur == num) {
131 |                 cout << "No motion filename specified; ignoring." << endl;
132 |                 continue;
133 |             }
134 |             out.motionname = args[cur++];
135 |             continue;
136 |         }
137 |         cout << "Unrecognized option: " << curStr << endl;
138 |         printUsageAndExit();
139 |     }
140 | 
141 |     return out;
142 | }
143 | 
144 | void process(const vector<string> &args, MyWindow *w)
145 | {
146 |     int i;
147 |     ArgData a = processArgs(args);
148 | 
149 |     Debugging::setOutStream(cout);
150 | 
151 |     Mesh m(a.filename);
152 |     if(m.vertices.size() == 0) {
153 |         cout << "Error reading file.  Aborting." << endl;
154 |         exit(0);
155 |         return;
156 |     }            
157 |     
158 |     for(i = 0; i < (int)m.vertices.size(); ++i)
159 |         m.vertices[i].pos = a.meshTransform * m.vertices[i].pos;
160 |     m.normalizeBoundingBox();
161 |     m.computeVertexNormals();
162 | 
163 |     Skeleton given = a.skeleton;
164 |     given.scale(a.skelScale * 0.7);
165 | 
166 |     if(a.stopAtMesh) { //if early bailout
167 |         w->addMesh(new StaticDisplayMesh(m));
168 |         return;
169 |     }
170 | 
171 |     PinocchioOutput o;
172 |     if(!a.noFit) { //do everything
173 |         o = autorig(given, m);
174 |     }
175 |     else { //skip the fitting step--assume the skeleton is already correct for the mesh
176 |         TreeType *distanceField = constructDistanceField(m);
177 |         VisTester<TreeType> *tester = new VisTester<TreeType>(distanceField);
178 | 
179 |         o.embedding = a.skeleton.fGraph().verts;
180 |         for(i = 0; i < (int)o.embedding.size(); ++i)
181 |             o.embedding[i] = m.toAdd + o.embedding[i] * m.scale;
182 | 
183 |         o.attachment = new Attachment(m, a.skeleton, o.embedding, tester);
184 | 
185 |         delete tester;
186 |         delete distanceField;
187 |     }
188 | 
189 |     if(o.embedding.size() == 0) {
190 |         cout << "Error embedding" << endl;
191 |         exit(0);
192 |     }
193 | 
194 |     if(a.motionname.size() > 0) {
195 |         w->addMesh(new DefMesh(m, given, o.embedding, *(o.attachment), new Motion(a.motionname)));
196 |     }
197 |     else {
198 |         w->addMesh(new StaticDisplayMesh(m));
199 | 
200 |         for(i = 1; i < (int)o.embedding.size(); ++i)
201 |         {
202 |             w->addLine(LineSegment(o.embedding[i], o.embedding[given.fPrev()[i]], Vector3(.5, .5, 0), 4.));
203 |         }
204 |     }
205 | 
206 |     //output skeleton embedding
207 |     for(i = 0; i < (int)o.embedding.size(); ++i)
208 |         o.embedding[i] = (o.embedding[i] - m.toAdd) / m.scale;
209 |     ofstream os("skeleton.out");
210 |     for(i = 0; i < (int)o.embedding.size(); ++i) {
211 |         os << i << " " << o.embedding[i][0] << " " << o.embedding[i][1] <<
212 |                    " " << o.embedding[i][2] << " " << a.skeleton.fPrev()[i] << endl;
213 |     }
214 | 
215 |     //output attachment
216 |     std::ofstream astrm("attachment.out");
217 |     for(i = 0; i < (int)m.vertices.size(); ++i) {
218 |         Vector<double, -1> v = o.attachment->getWeights(i);
219 |         for(int j = 0; j < v.size(); ++j) {
220 |             double d = floor(0.5 + v[j] * 10000.) / 10000.;
221 |             astrm << d << " ";
222 |         }
223 |         astrm << endl;
224 |     }
225 | 
226 |     delete o.attachment;
227 | }
228 | 


--------------------------------------------------------------------------------
/Pinocchio/discretization.cpp:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #include "pinocchioApi.h"
 20 | #include "deriv.h"
 21 | #include "debugging.h"
 22 | 
 23 | //fits mesh inside unit cube, makes sure there's exactly one connected component
 24 | Mesh  prepareMesh(const Mesh &m)
 25 | {
 26 |     Mesh out = m;
 27 | 
 28 |     if(!m.isConnected()) {
 29 |         Debugging::out() << "Bad mesh: should be a single connected component" << endl;
 30 |         return Mesh();
 31 |     }
 32 | 
 33 |     out.computeVertexNormals();
 34 |     out.normalizeBoundingBox();
 35 | 
 36 |     return out;
 37 | }
 38 | 
 39 | 
 40 | //constructs a distance field on an octree--user responsible for deleting output
 41 | TreeType *constructDistanceField(const Mesh &m, double tol)
 42 | {
 43 |     vector<Tri3Object> triobjvec;
 44 | //#pragma omp parallel for
 45 |     for(int i = 0; i < (int)m.edges.size(); i += 3) {
 46 |         Vector3 v1 = m.vertices[m.edges[i].vertex].pos;
 47 |         Vector3 v2 = m.vertices[m.edges[i + 1].vertex].pos;
 48 |         Vector3 v3 = m.vertices[m.edges[i + 2].vertex].pos;
 49 | 
 50 | //#pragma omp critical
 51 |         triobjvec.push_back(Tri3Object(v1, v2, v3));
 52 |     }
 53 |     
 54 |     ObjectProjector<3, Tri3Object> proj(triobjvec);
 55 | 
 56 |     TreeType *out = OctTreeMaker<TreeType>().make(proj, m, tol);
 57 | 
 58 |     Debugging::out() << "Done fullSplit " << out->countNodes() << " " << out->maxLevel() << endl;
 59 | 
 60 |     return out;
 61 | }
 62 | 
 63 | double getMinDot(TreeType *distanceField, const Vector3 &c, double step)
 64 | {
 65 |     typedef Deriv<double, 3> D;
 66 |     typedef Vector<D, 3> VD;
 67 |     
 68 |     int i, j;
 69 |     vector<Vector3> vecs;
 70 |     vecs.push_back(Vector3(step, step, step));
 71 |     vecs.push_back(Vector3(step, step, -step));
 72 |     vecs.push_back(Vector3(step, -step, step));
 73 |     vecs.push_back(Vector3(step, -step, -step));
 74 |     vecs.push_back(Vector3(-step, step, step));
 75 |     vecs.push_back(Vector3(-step, step, -step));
 76 |     vecs.push_back(Vector3(-step, -step, step));
 77 |     vecs.push_back(Vector3(-step, -step, -step));
 78 |     
 79 |     for(i = 0; i < (int)vecs.size(); ++i) {
 80 |         vecs[i] += c;
 81 |         VD vd = VD(D(vecs[i][0], 0), D(vecs[i][1], 1), D(vecs[i][2], 2));
 82 |         
 83 |         D result = distanceField->locate(vecs[i])->evaluate(vd);
 84 |         vecs[i] = Vector3(result.getDeriv(0), result.getDeriv(1), result.getDeriv(2)).normalize();
 85 |     }
 86 |     
 87 |     double minDot = 1.;
 88 |     
 89 |     for(i = 1; i < (int)vecs.size(); ++i) for(j = 0; j < i; ++j) {
 90 |         minDot = min(minDot, vecs[i] * vecs[j]);
 91 |     }
 92 |     
 93 |     return minDot;
 94 | }
 95 | 
 96 | bool sphereComp(const Sphere &s1, const Sphere &s2) { return s1.radius > s2.radius; }
 97 | 
 98 | //samples the distance field to find spheres on the medial surface
 99 | //output is sorted by radius in decreasing order
100 | vector<Sphere> sampleMedialSurface(TreeType *distanceField, double tol)
101 | {
102 |     int i;
103 |     vector<Sphere> out;
104 | 
105 |     vector<OctTreeNode *> todo;
106 |     todo.push_back(distanceField);
107 |     int inTodo = 0;
108 |     while(inTodo < (int)todo.size()) {
109 |         OctTreeNode *cur = todo[inTodo];
110 |         ++inTodo;
111 |         if(cur->getChild(0)) {
112 |             for(i = 0; i < 8; ++i) {
113 |                 todo.push_back(cur->getChild(i));
114 |             }
115 |             continue;
116 |         }
117 |         
118 |         //we are at octree leaf
119 |         Rect3 r = cur->getRect();
120 |         double rad = r.getSize().length() / 2.;
121 |         Vector3 c = r.getCenter();
122 |         double dot = getMinDot(distanceField, c, rad);
123 |         if(dot > 0.)
124 |             continue;
125 |     
126 |         //we are likely near medial surface
127 |         double step = tol;
128 |         double x, y;
129 |         vector<Vector3> pts;
130 |         double sz = r.getSize()[0];
131 |         for(x = 0; x <= sz; x += step) for(y = 0; y <= sz; y += step) {
132 |             pts.push_back(r.getLo() + Vector3(x, y, 0));
133 |             if(y != 0.)
134 |                 pts.push_back(r.getLo() + Vector3(x, 0, y));
135 |             if(x != 0. && y != 0.)
136 |                 pts.push_back(r.getLo() + Vector3(0, x, y));
137 |         }
138 |         
139 |         //pts now contains a grid on 3 of the octree cell faces (that's enough)
140 |         for(i = 0; i < (int)pts.size(); ++i) {
141 |             Vector3 &p = pts[i];
142 |             double dist = -distanceField->locate(p)->evaluate(p);
143 |             if(dist <= 2. * step)
144 |                 continue; //we want to be well inside
145 |             double dot = getMinDot(distanceField, p, step * 0.001);
146 |             if(dot > 0.0)
147 |                 continue;
148 |             out.push_back(Sphere(p, dist));
149 |         }
150 |     }
151 |     
152 |     Debugging::out() << "Medial axis points = " << out.size() << endl;
153 |     
154 |     sort(out.begin(), out.end(), sphereComp);
155 | 
156 |     return out;
157 | }
158 | 
159 | //takes sorted medial surface samples and sparsifies the vector
160 | vector<Sphere> packSpheres(const vector<Sphere> &samples, int maxSpheres)
161 | {
162 |     int i, j;
163 |     vector<Sphere> out;
164 | 
165 |     for(i = 0; i < (int)samples.size(); ++i) {
166 |         for(j = 0; j < (int)out.size(); ++j) {
167 |             if((out[j].center - samples[i].center).lengthsq() < SQR(out[j].radius))
168 |                     break;
169 |         }
170 |         if(j < (int)out.size())
171 |             continue;
172 | 
173 |         out.push_back(samples[i]);
174 |         if((int)out.size() > maxSpheres)
175 |             break;
176 |     }
177 | 
178 |     return out;
179 | }
180 | 
181 | double getMaxDist(TreeType *distanceField, const Vector3 &v1, const Vector3 &v2, double maxAllowed)
182 | {
183 |     double maxDist = -1e37;
184 |     Vector3 diff = (v2 - v1) / 100.;
185 |     for(int k = 0; k < 101; ++k) {
186 |         Vector3 pt = v1 + diff * double(k);
187 |         maxDist = max(maxDist, distanceField->locate(pt)->evaluate(pt));
188 |         if(maxDist > maxAllowed)
189 |             break;
190 |     }
191 |     return maxDist;
192 | }
193 | 
194 | //constructs graph on packed sphere centers
195 | PtGraph connectSamples(TreeType *distanceField, const vector<Sphere> &spheres)
196 | {
197 |     int i, j;
198 |     PtGraph out;
199 | 
200 |     for(i = 0; i < (int)spheres.size(); ++i)
201 |         out.verts.push_back(spheres[i].center);
202 |     out.edges.resize(spheres.size());
203 |         
204 |     for(i = 1; i < (int)spheres.size(); ++i) for(j = 0; j < i; ++j) {
205 |         int k;
206 |         Vector3 ctr = (spheres[i].center + spheres[j].center) * 0.5;
207 |         double radsq = (spheres[i].center - spheres[j].center).lengthsq() * 0.25;
208 |         if(radsq < SQR(spheres[i].radius + spheres[j].radius) * 0.25) { //if spheres intersect, there should be an edge
209 |             out.edges[i].push_back(j);
210 |             out.edges[j].push_back(i);
211 |             continue;
212 |         }
213 |         for(k = 0; k < (int)spheres.size(); ++k) {
214 |             if(k == i || k == j)
215 |                 continue;
216 |             if((spheres[k].center - ctr).lengthsq() < radsq)
217 |                 break; //gabriel graph condition violation
218 |         }
219 |         if(k < (int)spheres.size())
220 |             continue;
221 |         //every point on edge should be at least this far in:
222 |         double maxAllowed = -.5 * min(spheres[i].radius, spheres[j].radius);
223 |         if(getMaxDist(distanceField, spheres[i].center, spheres[j].center, maxAllowed) < maxAllowed) {
224 |             out.edges[i].push_back(j);
225 |             out.edges[j].push_back(i);
226 |         }
227 |     }
228 | 
229 |     return out;
230 | }
231 | 


--------------------------------------------------------------------------------
/Pinocchio/refinement.cpp:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #include "pinocchioApi.h"
 20 | #include "deriv.h"
 21 | #include "debugging.h"
 22 | 
 23 | 
 24 | struct RP //information for refined embedding
 25 | {
 26 |     RP(TreeType *inD, const Skeleton &inSk, const vector<Vector3> &medialSurface)
 27 |         : distanceField(inD), given(inSk)
 28 |     {
 29 |         vector<Vec3Object> mpts;
 30 |         for(int i = 0; i < (int)medialSurface.size(); ++i)
 31 |             mpts.push_back(medialSurface[i]);
 32 | 
 33 |         medProjector = ObjectProjector<3, Vec3Object>(mpts);
 34 |     }
 35 | 
 36 |     TreeType *distanceField;
 37 |     const Skeleton &given;
 38 |     ObjectProjector<3, Vec3Object> medProjector;
 39 | };
 40 | 
 41 | template<class Real> Real computeFineError(const vector<Vector<Real, 3> > &match, RP *rp)
 42 | {
 43 |     Real out = Real();
 44 |     int i;
 45 |     for(i = 1; i < (int)match.size(); ++i) {
 46 |         int prev = rp->given.fPrev()[i];
 47 |         
 48 |         Real surfPenalty = Real();
 49 |         Real lenPenalty = Real();
 50 |         Real anglePenalty = Real();
 51 |         Real symPenalty = Real();
 52 |         
 53 |         //-----------------surf
 54 |         const int samples = 10;
 55 |         for(int k = 0; k < samples; ++k) {
 56 |             double frac = double(k) / double(samples);
 57 |             Vector<Real, 3> cur = match[i] * Real(1. - frac) + match[prev] * Real(frac);
 58 |             Vector3 m = rp->medProjector.project(cur);
 59 |             Real medDist = (cur - Vector<Real, 3>(m)).length();
 60 |             Real surfDist = -rp->distanceField->locate(cur)->evaluate(cur);
 61 |             Real penalty = SQR(min(medDist, Real(0.001) + max(Real(0.), Real(0.05) - surfDist)));
 62 |             if(penalty > Real(SQR(0.003)))
 63 |                 surfPenalty += Real(1. / double(samples)) * penalty;
 64 |         }
 65 |         
 66 |         //---------------length
 67 |         Real optDistSq = (rp->given.fGraph().verts[i] - rp->given.fGraph().verts[prev]).lengthsq();
 68 |         Real distSq = SQR(max(Real(-10.), (match[i] - match[prev]) *
 69 |                                         (rp->given.fGraph().verts[i] - rp->given.fGraph().verts[prev]))) / optDistSq;
 70 |         lenPenalty = max(Real(.5), (Real(0.0001) + optDistSq) / (Real(0.0001) + distSq));
 71 |         
 72 |         //---------------sym
 73 |         if(rp->given.fSym()[i] != -1) {
 74 |             int s = rp->given.fSym()[i];
 75 |             int sp = rp->given.fPrev()[s];
 76 |             
 77 |             Real sDistSq = (match[s] - match[sp]).lengthsq();
 78 |             symPenalty = max(Real(1.05), max(distSq / (Real(0.001) + sDistSq), sDistSq / (Real(0.001) + distSq)));
 79 |         }
 80 |         
 81 |         //--------------angle
 82 |         if(distSq > Real(1e-16)) {
 83 |             Vector<Real, 3> curDir = (match[i] - match[prev]).normalize();
 84 |             Vector<Real, 3> skelDir = (rp->given.fGraph().verts[i] - rp->given.fGraph().verts[prev]).normalize();
 85 |             if(curDir * skelDir < Real(1. - 1e-8))
 86 |                 anglePenalty = Real(0.5) * acos(curDir * skelDir);
 87 |             anglePenalty = CUBE(Real(0.3) + anglePenalty);
 88 |             if(curDir * skelDir < Real(0.))
 89 |                 anglePenalty *= 10.;
 90 |         }
 91 |         
 92 |         out += Real(15000.) * surfPenalty + Real(0.25) * lenPenalty + Real(2.0) * anglePenalty + symPenalty;
 93 |     }
 94 |     
 95 |     return out;
 96 | }
 97 | 
 98 | vector<Vector3> optimizeEmbedding1D(vector<Vector3> fineEmbedding, vector<Vector3> dir, RP *rp)
 99 | {
100 |     int i;
101 |     double step = 0.001;
102 |     
103 |     for(i = 0; i < (int)fineEmbedding.size(); ++i) {
104 |         step += dir[i].lengthsq();
105 |     }
106 |     
107 |     step = 0.0005 / sqrt(step);
108 |     
109 |     double prevErr = -1e10;
110 |     int count = 0;
111 |     while(++count) {
112 |         double curErr = computeFineError(fineEmbedding, rp);
113 |         if(prevErr == -1e10 || curErr < prevErr) {
114 |             step *= 2.;
115 |             for(i = 0; i < (int)fineEmbedding.size(); ++i) {
116 |                 fineEmbedding[i] += dir[i] * step;
117 |             }
118 |             prevErr = curErr;
119 |         }
120 |         else {
121 |             if(count > 2) {
122 |                 for(i = 0; i < (int)fineEmbedding.size(); ++i) {
123 |                     fineEmbedding[i] -= dir[i] * step;
124 |                 }
125 |             }
126 |             break;
127 |         }
128 |     }
129 |     
130 |     return fineEmbedding;
131 | }
132 | 
133 | 
134 | 
135 | //refines embedding
136 | vector<Vector3> refineEmbedding(TreeType *distanceField, const vector<Vector3> &medialSurface,
137 |                                 const vector<Vector3> &initialEmbedding, const Skeleton &skeleton)
138 | {
139 |     RP rp(distanceField, skeleton, medialSurface);
140 | 
141 |     int sz = initialEmbedding.size();
142 |     vector<Vector3> fineEmbedding = initialEmbedding;
143 |     int i, k;
144 |     for(k = 0; k < 10; ++k) {
145 |         typedef Deriv<double, 6> DType;
146 |         typedef Deriv<double, -1> DType1;
147 |         
148 |         Debugging::out() << "E = " << computeFineError(fineEmbedding, &rp) << endl;
149 |         
150 |         for(int j = 0; j < 2; ++j) {
151 |             vector<Vector<DType1, 3> > dMatch(sz);
152 |             for(i = 0; i < sz; ++i) {
153 |                 dMatch[i][0] = DType1(fineEmbedding[i][0], i * 3);
154 |                 dMatch[i][1] = DType1(fineEmbedding[i][1], i * 3 + 1);
155 |                 dMatch[i][2] = DType1(fineEmbedding[i][2], i * 3 + 2);
156 |             }
157 | 
158 |             DType1 err = computeFineError(dMatch, &rp) + (DType1() * DType1(0., 3 * sz));
159 |             vector<Vector3> dir(sz);
160 |         
161 |             for(i = 0; i < sz; ++i) {
162 |                 dir[i][0] = -err.getDeriv(i * 3);
163 |                 dir[i][1] = -err.getDeriv(i * 3 + 1);
164 |                 dir[i][2] = -err.getDeriv(i * 3 + 2);
165 |             }
166 |             fineEmbedding = optimizeEmbedding1D(fineEmbedding, dir, &rp);
167 |         }
168 |         
169 |         int cur;
170 |         for(cur = 1; cur < sz; ++cur) {
171 |             int prev = skeleton.fPrev()[cur];
172 |             
173 |             vector<Vector<DType, 3> > dMatch(sz);
174 |             for(i = 0; i < sz; ++i) {
175 |                 dMatch[i][0] = DType(fineEmbedding[i][0]);
176 |                 dMatch[i][1] = DType(fineEmbedding[i][1]);
177 |                 dMatch[i][2] = DType(fineEmbedding[i][2]);
178 |                 
179 |                 int varNum = -1;
180 |                 if(i == cur)
181 |                     varNum = 0;
182 |                 else if(i == prev)
183 |                     varNum = 3;
184 |                 
185 |                 if(varNum >= 0) {
186 |                     dMatch[i][0] *= DType(1, varNum + 0);
187 |                     dMatch[i][1] *= DType(1, varNum + 1);
188 |                     dMatch[i][2] *= DType(1, varNum + 2);
189 |                 }
190 |             }
191 |             
192 |             DType err = computeFineError(dMatch, &rp);
193 |             
194 |             vector<Vector3> dir(sz);
195 |         
196 |             for(i = 0; i < sz; ++i) {
197 |                 int varNum = -1;
198 |                 if(i == cur)
199 |                     varNum = 0;
200 |                 else if(i == prev)
201 |                     varNum = 3;
202 |                 
203 |                 if(varNum >= 0) {
204 |                     dir[i][0] = -err.getDeriv(varNum);
205 |                     dir[i][1] = -err.getDeriv(varNum + 1);
206 |                     dir[i][2] = -err.getDeriv(varNum + 2);
207 |                 }
208 |             }
209 |             fineEmbedding = optimizeEmbedding1D(fineEmbedding, dir, &rp);
210 |         }
211 |     }
212 |     
213 |     return fineEmbedding;
214 | }
215 | 
216 | 


--------------------------------------------------------------------------------
/Pinocchio/transform.h:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #ifndef TRANSFORM_H
 20 | #define TRANSFORM_H
 21 | 
 22 | #include "vector.h"
 23 | 
 24 | template<class Real = double>
 25 | class Quaternion //normalized quaternion for representing rotations
 26 | {
 27 | public:
 28 |     //constructors
 29 |     Quaternion() : r(1.) { } //initialize to identity
 30 |     Quaternion(const Quaternion &q) : r(q.r), v(q.v) {} //copy constructor
 31 |     template<class R> Quaternion(const Quaternion<R> &q) : r(q.r), v(q.v) {} //convert quaternions of other types
 32 |     //axis angle constructor:
 33 |     template<class R> Quaternion(const Vector<R, 3> &axis, const R &angle) : r(cos(angle * Real(0.5))), v(sin(angle * Real(0.5)) * axis.normalize()) {}
 34 |     //minimum rotation constructor:
 35 |     template<class R> Quaternion(const Vector<R, 3> &from, const Vector<R, 3> &to) : r(1.)
 36 |     {
 37 |         R fromLenSq = from.lengthsq(), toLenSq = to.lengthsq();
 38 |         if(fromLenSq < toLenSq) {
 39 |             if(fromLenSq < R(1e-16))
 40 |                 return;
 41 |             Vector<R, 3> mid = from * sqrt(toLenSq / fromLenSq) + to;
 42 |             R fac = 1. / sqrt(mid.lengthsq() * toLenSq);
 43 |             r = (mid * to) * fac;
 44 |             v = (mid % to) * fac;
 45 |         }
 46 |         else {
 47 |             if(toLenSq < R(1e-16))
 48 |                 return;
 49 |             Vector<R, 3> mid = from + to * sqrt(fromLenSq / toLenSq);
 50 |             R fac = 1. / sqrt(mid.lengthsq() * fromLenSq);
 51 |             r = (from * mid) * fac;
 52 |             v = (from % mid) * fac;
 53 |         }
 54 |     }
 55 | 
 56 |     //quaternion multiplication
 57 |     Quaternion operator*(const Quaternion &q) const { return Quaternion(r * q.r - v * q.v, r * q.v + q.r * v + v % q.v); }
 58 |     
 59 |     //transforming a vector
 60 |     Vector<Real, 3> operator*(const Vector<Real, 3> &p) const
 61 |     {
 62 |         Vector<Real, 3> v2 = v + v;
 63 |         Vector<Real, 3> vsq2 = v.apply(multiplies<Real>(), v2);
 64 |         Vector<Real, 3> rv2 = r * v2;
 65 |         Vector<Real, 3> vv2(v[1] * v2[2], v[0] * v2[2], v[0] * v2[1]);
 66 |         return Vector<Real, 3>(p[0] * (Real(1.) - vsq2[1] - vsq2[2]) + p[1] * (vv2[2] - rv2[2]) + p[2] * (vv2[1] + rv2[1]),
 67 |                                p[1] * (Real(1.) - vsq2[2] - vsq2[0]) + p[2] * (vv2[0] - rv2[0]) + p[0] * (vv2[2] + rv2[2]),
 68 |                                p[2] * (Real(1.) - vsq2[0] - vsq2[1]) + p[0] * (vv2[1] - rv2[1]) + p[1] * (vv2[0] + rv2[0]));
 69 |     }
 70 | 
 71 |     //equality
 72 |     template<class R> bool operator==(const Quaternion<R> &oth) const
 73 |     {
 74 |         return (r == oth.r && v == oth.v) || (r == -oth.r && v == -oth.v);
 75 |     }
 76 |     
 77 |     Quaternion inverse() const { return Quaternion(-r, v); }
 78 |     
 79 |     Real getAngle() const { return Real(2.) * atan2(v.length(), r); }
 80 |     Vector<Real, 3> getAxis() const { return v.normalize(); }
 81 |     
 82 |     const Real &operator[](int i) const { return (i == 0) ? r : v[i - 1]; }
 83 |     void set(const Real &inR, const Vector<Real, 3> &inV) {
 84 |         Real ratio = Real(1.) / sqrt(inR * inR + inV.lengthsq()); 
 85 |         r = inR * ratio; v = inV * ratio; //normalize
 86 |     }
 87 |     
 88 | private:
 89 |     Quaternion(const Real &inR, const Vector<Real, 3> &inV) : r(inR), v(inV) {}
 90 |     
 91 |     Real r;
 92 |     Vector<Real, 3> v;
 93 | };
 94 | 
 95 | template<class Real = double> class Transform { //T(v) = (rot * v * scale) + trans
 96 | public:
 97 |     typedef Vector<Real, 3> Vec;
 98 |     
 99 |     Transform() : scale(1.) {}
100 |     explicit Transform(const Real &inScale) : scale(inScale) {}
101 |     explicit Transform(const Vec &inTrans) : scale(1.), trans(inTrans) {}
102 |     Transform(const Quaternion<Real> &inRot, Real inScale = Real(1.), Vec inTrans = Vec()) : rot(inRot), scale(inScale), trans(inTrans) {}
103 |     Transform(const Transform &t) : rot(t.rot), scale(t.scale), trans(t.trans) {}
104 |     
105 |     Transform operator*(const Transform &t) const { return Transform(rot * t.rot, scale * t.scale, trans + rot * (scale * t.trans)); }
106 |     Vec operator*(const Vec &v) const { return rot * (v * scale) + trans; }
107 |     
108 |     Transform inverse() const { return Transform(rot.inverse(), 1. / scale, rot.inverse() * -trans * (1. / scale)); }
109 |     
110 |     Transform linearComponent() const { return Transform(rot, scale); }
111 |     Vec mult3(const Vec &v) const { return rot * (v * scale); }
112 |     
113 |     Real getScale() const { return scale; }
114 |     Vec getTrans() const { return trans; }
115 |     Quaternion<Real> getRot() const { return rot; }
116 |     
117 | private:
118 |     Quaternion<Real> rot;
119 |     Real scale;
120 |     Vec trans;
121 | };
122 | 
123 | template<class Real = double> class Matrix3 {
124 | public:
125 |     typedef Vector<Real, 3> Vec;
126 |     typedef Matrix3<Real> Self;
127 |     
128 |     Matrix3(const Real &diag = Real()) { m[0] = m[4] = m[8] = diag; m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = Real(); }
129 |     Matrix3(const Vec &c1, const Vec &c2, const Vec &c3) {
130 |         m[0] = c1[0]; m[1] = c2[0]; m[2] = c3[0];
131 |         m[3] = c1[1]; m[4] = c2[1]; m[5] = c3[1];
132 |         m[6] = c1[2]; m[7] = c2[2]; m[8] = c3[2];
133 |     }
134 |     Matrix3(const Self &inM) { for(int i = 0; i < 9; ++i) m[i] = inM[i]; }
135 |     
136 |     Real &operator[](int idx) { return m[idx]; }
137 |     const Real &operator[](int idx) const { return m[idx]; }
138 |     Real &operator()(int row, int col) { return m[row * 3 + col]; }
139 |     const Real &operator()(int row, int col) const { return m[row * 3 + col]; }
140 |     
141 |     Vec getRow(int row) const { row *= 3; return Vec(m[row], m[row + 1], m[row + 2]); }
142 |     Vec getColumn(int col) const { return Vec(m[col], m[col + 3], m[col + 6]); }
143 |     
144 |     Self operator+(const Self &o) { Self out(S(0)); for(int i = 0; i < 9; ++i) out[i] = m[i] + o[i]; return out; }
145 |     Self operator-(const Self &o) { Self out(S(0)); for(int i = 0; i < 9; ++i) out[i] = m[i] - o[i]; return out; }
146 |     Self operator*(const Real &x) { Self out(S(0)); for(int i = 0; i < 9; ++i) out[i] = m[i] * x; return out; }
147 |     Self operator/(const Real &x) { Self out(S(0)); for(int i = 0; i < 9; ++i) out[i] = m[i] / x; return out; }
148 | 
149 | #define OPAS(op, typ, idx) Self &operator op(const typ &x) { for(int i = 0; i < 9; ++i) m[i] op x idx; return *this; }
150 |     OPAS(+=, Self, [i])
151 |     OPAS(-=, Self, [i])
152 |     OPAS(*=, Real, )
153 |     OPAS(/=, Real, )
154 | #undef OPAS
155 |     
156 |     Vec operator*(const Vec &v) const { 
157 |         return Vec(m[0] * v[0] + m[1] * v[1] + m[2] * v[2],
158 |                    m[3] * v[0] + m[4] * v[1] + m[5] * v[2],
159 |                    m[6] * v[0] + m[7] * v[1] + m[8] * v[2]);
160 |     }
161 |     
162 |     Self operator*(const Self &o) const {
163 |         return Self((*this) * Vec(o[0], o[3], o[6]), (*this) * Vec(o[1], o[4], o[7]), (*this) * Vec(o[2], o[5], o[8]));
164 |     }
165 |     
166 |     Self operator~() const { //transpose
167 |         Self out(S(0)); //uninitialized
168 |         out[0] = m[0]; out[4] = m[4]; out[8] = m[8];
169 |         out[1] = m[3]; out[3] = m[1]; out[2] = m[6]; out[6] = m[2]; out[5] = m[7]; out[7] = m[5];
170 |         return out;
171 |     }
172 |     
173 |     Self operator!() const { //invert
174 |         Self out(S(0));
175 |         Real d = det();
176 |         if(d == Real())
177 |             return Self();
178 |         d = Real(1.) / d;
179 |         out[0] = d * (m[4] * m[8] - m[5] * m[7]);
180 |         out[1] = d * (m[2] * m[7] - m[1] * m[8]);
181 |         out[2] = d * (m[1] * m[5] - m[2] * m[4]);
182 |         out[3] = d * (m[5] * m[6] - m[3] * m[8]);
183 |         out[4] = d * (m[0] * m[8] - m[2] * m[6]);
184 |         out[5] = d * (m[2] * m[3] - m[0] * m[5]);
185 |         out[6] = d * (m[3] * m[7] - m[4] * m[6]);
186 |         out[7] = d * (m[1] * m[6] - m[0] * m[7]);
187 |         out[8] = d * (m[0] * m[4] - m[1] * m[3]);
188 |         return out;
189 |     }
190 |     
191 |     Real det() const
192 |     { return m[0] * (m[4] * m[8] - m[5] * m[7]) - m[1] * (m[3] * m[8] - m[5] * m[6]) + m[2] * (m[3] * m[7] - m[4] * m[6]); }
193 |     
194 | private:
195 |     struct S { S(int) { } };
196 |     Matrix3(const S &) { } //no initialization
197 |     
198 |     Real m[9];
199 | };
200 | 
201 | template <class charT, class traits, class Real>
202 |         basic_ostream<charT,traits>& operator<<(basic_ostream<charT,traits>& os, const Matrix3<Real> &m)
203 | {
204 |     os << "[[" << m[0] << "," << m[1] << "," << m[2] << "]";
205 |     os << "[" << m[3] << "," << m[4] << "," << m[5] << "]";
206 |     os << "[" << m[6] << "," << m[7] << "," << m[8] << "]]";
207 |     return os;
208 | }
209 | 
210 | #endif
211 | 


--------------------------------------------------------------------------------
/Pinocchio/attachment.cpp:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #include <fstream>
 20 | #include <sstream>
 21 | #include "attachment.h"
 22 | #include "vecutils.h"
 23 | #include "lsqSolver.h"
 24 | 
 25 | class AttachmentPrivate
 26 | {
 27 | public:
 28 |     AttachmentPrivate() {}
 29 |     virtual ~AttachmentPrivate() {}
 30 |     virtual Mesh deform(const Mesh &mesh, const vector<Transform<> > &transforms) const = 0;
 31 |     virtual Vector<double, -1> getWeights(int i) const = 0;
 32 |     virtual AttachmentPrivate *clone() const = 0;
 33 | };
 34 |     
 35 | bool vectorInCone(const Vector3 &v, const vector<Vector3> &ns)
 36 | {
 37 |     int i;
 38 |     Vector3 avg;
 39 |     for(i = 0; i < (int)ns.size(); ++i)
 40 |         avg += ns[i];
 41 |         
 42 |     return v.normalize() * avg.normalize() > 0.5;
 43 | }
 44 | 
 45 | class AttachmentPrivate1 : public AttachmentPrivate {
 46 | public:
 47 |     AttachmentPrivate1() {}
 48 | 
 49 |     AttachmentPrivate1(const Mesh &mesh, const Skeleton &skeleton, const vector<Vector3> &match, const VisibilityTester *tester)
 50 |     {
 51 |         int i, j;
 52 |         int nv = mesh.vertices.size();
 53 |         //compute edges
 54 |         vector<vector<int> > edges(nv);
 55 | 
 56 |         for(i = 0; i < nv; ++i) {
 57 |             int cur, start;
 58 |             cur = start = mesh.vertices[i].edge;
 59 |             do {
 60 |                 edges[i].push_back(mesh.edges[cur].vertex);
 61 |                 cur = mesh.edges[mesh.edges[cur].prev].twin;
 62 |             } while(cur != start);
 63 |         }
 64 |     
 65 |         weights.resize(nv);
 66 |         int bones = skeleton.fGraph().verts.size() - 1;
 67 | 
 68 |         for(i = 0; i < nv; ++i) // initialize the weights vectors so they are big enough
 69 |             weights[i][bones - 1] = 0.;
 70 | 
 71 |         vector<vector<double> > boneDists(nv);
 72 |         vector<vector<bool> > boneVis(nv);
 73 |         
 74 |         for(i = 0; i < nv; ++i) {
 75 |             boneDists[i].resize(bones, -1);
 76 |             boneVis[i].resize(bones);
 77 |             Vector3 cPos = mesh.vertices[i].pos;
 78 |                 
 79 |             vector<Vector3> normals;
 80 |             for(j = 0; j < (int)edges[i].size(); ++j) {
 81 |                 int nj = (j + 1) % edges[i].size();
 82 |                 Vector3 v1 = mesh.vertices[edges[i][j]].pos - cPos;
 83 |                 Vector3 v2 = mesh.vertices[edges[i][nj]].pos - cPos;
 84 |                 normals.push_back((v1 % v2).normalize());
 85 |             }
 86 |                     
 87 |             double minDist = 1e37;
 88 |             for(j = 1; j <= bones; ++j) {
 89 |                 const Vector3 &v1 = match[j], &v2 = match[skeleton.fPrev()[j]];
 90 |                 boneDists[i][j - 1] = sqrt(distsqToSeg(cPos, v1, v2));
 91 |                 minDist = min(boneDists[i][j - 1], minDist);
 92 |             }
 93 |             for(j = 1; j <= bones; ++j) {
 94 |                 //the reason we don't just pick the closest bone is so that if two are
 95 |                 //equally close, both are factored in.
 96 |                 if(boneDists[i][j - 1] > minDist * 1.0001)
 97 |                     continue;
 98 | 
 99 |                 const Vector3 &v1 = match[j], &v2 = match[skeleton.fPrev()[j]];
100 |                 Vector3 p = projToSeg(cPos, v1, v2);
101 |                 boneVis[i][j - 1] = tester->canSee(cPos, p) && vectorInCone(cPos - p, normals);
102 |             }
103 |         }
104 | 
105 |         //We have -Lw+Hw=HI, same as (H-L)w=HI, with (H-L)=DA (with D=diag(1./area))
106 |         //so w = A^-1 (HI/D)
107 | 
108 |         vector<vector<pair<int, double> > > A(nv);
109 |         vector<double> D(nv, 0.), H(nv, 0.);
110 |         vector<int> closest(nv, -1);
111 |         for(i = 0; i < nv; ++i) {
112 |             //get areas
113 |             for(j = 0; j < (int)edges[i].size(); ++j) {
114 |                 int nj = (j + 1) % edges[i].size();
115 |         
116 |                 D[i] += ((mesh.vertices[edges[i][j]].pos - mesh.vertices[i].pos) %
117 |                          (mesh.vertices[edges[i][nj]].pos - mesh.vertices[i].pos)).length();
118 |             }
119 |             D[i] = 1. / (1e-10 + D[i]);
120 | 
121 |             //get bones
122 |             double minDist = 1e37;
123 |             for(j = 0; j < bones; ++j) {
124 |                 if(boneDists[i][j] < minDist) {
125 |                     closest[i] = j;
126 |                     minDist = boneDists[i][j];
127 |                 }
128 |             }
129 |             for(j = 0; j < bones; ++j)
130 |                 if(boneVis[i][j] && boneDists[i][j] <= minDist * 1.00001)
131 |                     H[i] += 1. / SQR(1e-8 + boneDists[i][closest[i]]);
132 | 
133 |             //get laplacian
134 |             double sum = 0.;
135 |             for(j = 0; j < (int)edges[i].size(); ++j) {
136 |                 int nj = (j + 1) % edges[i].size();
137 |                 int pj = (j + edges[i].size() - 1) % edges[i].size();
138 | 
139 |                 Vector3 v1 = mesh.vertices[i].pos - mesh.vertices[edges[i][pj]].pos;
140 |                 Vector3 v2 = mesh.vertices[edges[i][j]].pos - mesh.vertices[edges[i][pj]].pos;
141 |                 Vector3 v3 = mesh.vertices[i].pos - mesh.vertices[edges[i][nj]].pos;
142 |                 Vector3 v4 = mesh.vertices[edges[i][j]].pos - mesh.vertices[edges[i][nj]].pos;
143 |                 
144 |                 double cot1 = (v1 * v2) / (1e-6 + (v1 % v2).length());
145 |                 double cot2 = (v3 * v4) / (1e-6 + (v3 % v4).length());
146 |                 sum += (cot1 + cot2);
147 | 
148 |                 if(edges[i][j] > i) //check for triangular here because sum should be computed regardless
149 |                     continue;
150 |                 A[i].push_back(make_pair(edges[i][j], -cot1 - cot2));
151 |             }
152 | 
153 |             A[i].push_back(make_pair(i, sum + H[i] / D[i]));
154 | 
155 |             sort(A[i].begin(), A[i].end());
156 |         }
157 | 
158 |         nzweights.resize(nv);
159 |         SPDMatrix Am(A);
160 |         LLTMatrix *Ainv = Am.factor();
161 |         if(Ainv == NULL)
162 |             return;
163 | 
164 |         for(j = 0; j < bones; ++j) {
165 |             vector<double> rhs(nv, 0.);
166 |             for(i = 0; i < nv; ++i) {
167 |                 if(boneVis[i][j] && boneDists[i][j] <= boneDists[i][closest[i]] * 1.00001)
168 |                     rhs[i] = H[i] / D[i];
169 |             }
170 | 
171 |             Ainv->solve(rhs);           
172 |             for(i = 0; i < nv; ++i) {
173 |                 if(rhs[i] > 1.)
174 |                     rhs[i] = 1.; //clip just in case
175 |                 if(rhs[i] > 1e-8)
176 |                     nzweights[i].push_back(make_pair(j, rhs[i]));
177 |             }
178 |         }
179 | 
180 |         for(i = 0; i < nv; ++i) {
181 |             double sum = 0.;
182 |             for(j = 0; j < (int)nzweights[i].size(); ++j)
183 |                 sum += nzweights[i][j].second;
184 | 
185 |             for(j = 0; j < (int)nzweights[i].size(); ++j) {
186 |                 nzweights[i][j].second /= sum;
187 |                 weights[i][nzweights[i][j].first] = nzweights[i][j].second;
188 |             }
189 |         }
190 | 
191 |         delete Ainv;
192 |         return;
193 |     }
194 | 
195 |     Mesh deform(const Mesh &mesh, const vector<Transform<> > &transforms) const
196 |     {
197 |         Mesh out = mesh;
198 |         int i, nv = mesh.vertices.size();
199 | 
200 |         if(mesh.vertices.size() != weights.size())
201 |             return out; //error
202 | 
203 |         for(i = 0; i < nv; ++i) {
204 |             Vector3 newPos;
205 |             int j;
206 |             for(j = 0; j < (int)nzweights[i].size(); ++j) {
207 |                 newPos += ((transforms[nzweights[i][j].first] * out.vertices[i].pos) * nzweights[i][j].second);
208 |             }
209 |             out.vertices[i].pos = newPos;
210 |         }
211 |         
212 |         out.computeVertexNormals();
213 |     
214 |         return out;
215 |     }
216 | 
217 |     Vector<double, -1> getWeights(int i) const { return weights[i]; }
218 | 
219 |     AttachmentPrivate *clone() const
220 |     {
221 |         AttachmentPrivate1 *out = new AttachmentPrivate1();
222 |         *out = *this;
223 |         return out;
224 |     }
225 | 
226 | private:
227 |     vector<Vector<double, -1> > weights;
228 |     vector<vector<pair<int, double> > > nzweights; //sparse representation
229 | };
230 | 
231 | Attachment::~Attachment()
232 | {
233 |     if(a)
234 |         delete a;   
235 | }
236 | 
237 | Attachment::Attachment(const Attachment &att)
238 | {
239 |     a = att.a->clone();
240 | }
241 | 
242 | Vector<double, -1> Attachment::getWeights(int i) const { return a->getWeights(i); }
243 | 
244 | Mesh Attachment::deform(const Mesh &mesh, const vector<Transform<> > &transforms) const
245 | {
246 |     return a->deform(mesh, transforms);
247 | }
248 | 
249 | Attachment::Attachment(const Mesh &mesh, const Skeleton &skeleton, const vector<Vector3> &match, const VisibilityTester *tester)
250 | {
251 |     a = new AttachmentPrivate1(mesh, skeleton, match, tester);
252 | }
253 | 


--------------------------------------------------------------------------------
/KinectSkeletonRigger.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  *  KinectSkeletonRigger.cpp
  3 |  *  KinectPuppeteering
  4 |  *
  5 |  *  Created by Roy Shilkrot on 11/25/12.
  6 |  *  Copyright 2012 MIT. All rights reserved.
  7 |  *
  8 |  */
  9 | 
 10 | #include "KinectSkeletonRigger.h"
 11 | 
 12 | void KinectSkeletonRigger::AutoRig() {
 13 | 	if (m_m.vertices.size() <= 0 || m_skel.get() == NULL) {
 14 | 		return;
 15 | 	}
 16 | 	cout << "autorigging...";
 17 | 	Debugging::setOutStream(cout);
 18 | 	KinectSkeleton tmp_skel = *m_skel;
 19 | 	m_po = autorig(tmp_skel, m_m);
 20 | 	cout << "DONE" << endl;
 21 | 	
 22 | 	if(m_po.attachment != NULL) {
 23 | 		//		m_display_mesh = m_po.attachment->deform(m_m,m_transforms);	
 24 | 		
 25 | 		my_weights.resize(m_m.vertices.size());
 26 | 		for (int i=0; i<m_m.vertices.size(); i++) {
 27 | 			my_weights[i] = m_po.attachment->getWeights(i);
 28 | 		}
 29 | 	}		
 30 | }
 31 | 
 32 | void KinectSkeletonRigger::ComputeBoneTransforms(const EigT3f& globalT) {
 33 | 	if(my_weights.size() > 0) {
 34 | //		float minpos = m_pose->jointV[extremes[1]].y();
 35 | 		
 36 | //		float foot_tras = (minpos - 0.1) * embedding_scale["lankle"];
 37 | 		
 38 | 		Ts.resize(m_skel->fGraph().verts.size(),Eigen::Affine3f::Identity());
 39 | 		for (int v_idx = 0; v_idx < m_skel->fGraph().verts.size(); v_idx++) 
 40 | 		{
 41 | 			int prev_idx = m_skel->fPrev()[v_idx];
 42 | 			assert(m_pose->jointIdxToName[v_idx] == m_skel->jointIdxToName[v_idx]);
 43 | 			string joint_name = m_skel->jointIdxToName[v_idx];
 44 | 			if (joint_name == "shoulders") {
 45 | 				prev_idx = m_skel->getJointForName("back");
 46 | 			}
 47 | 			int prev_prev_idx = m_skel->fPrev()[prev_idx];
 48 | 			string prev_joint_name = m_skel->jointIdxToName[prev_idx];
 49 | 			if (prev_joint_name == "shoulders") {
 50 | 				prev_prev_idx = m_skel->getJointForName("back");
 51 | 			}
 52 | 			string prev_prev_joint_name = m_skel->jointIdxToName[prev_prev_idx];
 53 | 			
 54 | 			cout << prev_prev_joint_name << " -> " << prev_joint_name << " -> " << joint_name << endl;
 55 | 			
 56 | 			EigV3f pose_prevprev_to_prev = (m_pose->jointV[prev_joint_name]-m_pose->jointV[prev_prev_joint_name]);
 57 | 			EigV3f skel_prevprev_to_prev = (m_skel->jointV[prev_joint_name]-m_skel->jointV[prev_prev_joint_name]);
 58 | 			EigV3f trns_skel_to_pose = (pose_prevprev_to_prev - skel_prevprev_to_prev) * embedding_scale[prev_joint_name];
 59 | 
 60 | 			EigQuat q_skel_to_pose = EigQuat::Identity().setFromTwoVectors(m_skel->referenceFrames[joint_name].row(0),
 61 | 															  m_pose->referenceFrames[joint_name].row(0));
 62 | 			
 63 | 			EigV3f embed = Pinoc2EigV3f(m_po.embedding[v_idx]);
 64 | 			EigV3f embed_prev = Pinoc2EigV3f(m_po.embedding[prev_idx]);
 65 | 			EigV3f embed_prev_prev = Pinoc2EigV3f(m_po.embedding[prev_prev_idx]);
 66 | 
 67 | 			EigQuat q_embed_to_skel = EigQuat::Identity().setFromTwoVectors((embed_prev - embed).normalized(),
 68 | 																		   m_skel->referenceFrames[joint_name].row(0));
 69 | 			
 70 | //			double embed_bone = (bone_offset-prev_bone_offset).norm();
 71 | 			EigV3f embed_prevprev_to_prev = embed_prev - embed_prev_prev;
 72 | 			EigV3f trns_embed_to_skel = (skel_prevprev_to_prev * embedding_scale[prev_joint_name] - embed_prevprev_to_prev);
 73 | 			
 74 | 			Ts[v_idx] = globalT * 
 75 | //					Eigen::Translation3f(EigV3f(0,-foot_tras,0)) *  // make sit on lowest joint
 76 | //				Eigen::Translation3f(-Pinoc2EigV3f(m_po.embedding[m_pose->getJointForName("back")])) * 
 77 | 				Eigen::Translation3f(trns_skel_to_pose) *
 78 | 				Eigen::Translation3f(trns_embed_to_skel) *
 79 | 				Eigen::Translation3f(embed_prev) *
 80 | 				q_skel_to_pose *
 81 | 				q_embed_to_skel *
 82 | 				Eigen::Translation3f(-embed_prev);
 83 | 			
 84 | 			//			cout << "transform " << joint_name << ": " << endl << Ts[v_idx].affine() << endl;
 85 | 		}
 86 | 		
 87 | 		//apply trasforms
 88 | 		//TODO: parallelize / GPU
 89 | 		m_display_mesh = m_m;
 90 | 		for (int vert_i=0; vert_i < m_display_mesh.vertices.size(); vert_i++) {
 91 | 			EigV3f vert_pos = Pinoc2EigV3f(m_display_mesh.vertices[vert_i].pos);
 92 | 			EigV3f new_vert_pos = EigV3f::Zero();
 93 | 			Vector<double, -1> weights = my_weights[vert_i];
 94 | 			assert(weights.size() == Ts.size() - 1);
 95 | 			
 96 | 			float w_sum = 0.0f;
 97 | 			for (int bone_i=1; bone_i < Ts.size(); bone_i++) {
 98 | 				float weight = weights[bone_i-1];
 99 | 				if (weight > 1.) weight = 1.0;
100 | 				//				if(weight < 1e-8) continue;
101 | 				
102 | 				new_vert_pos += (Ts[bone_i] * vert_pos) * weight; //Linear blend skinning
103 | 			}
104 | 			
105 | 			m_display_mesh.vertices[vert_i].pos = Eig2PinocV3f(new_vert_pos);
106 | 		}
107 | 	}
108 | }
109 | 
110 | void KinectSkeletonRigger::drawSkeleton(const KinectSkeleton::Ptr& _skel, bool draw_frames) {
111 | 	if (_skel.get() != NULL) {
112 | 		_skel->glDraw(/*globalTransform*/EigT3f::Identity(),draw_frames);
113 | 	}
114 | }
115 | 
116 | void KinectSkeletonRigger::drawEmbeddingSkel() {
117 | 	if (m_skel.get() != NULL) {
118 | 		glDisable(GL_DEPTH_TEST);
119 | 		glDisable(GL_LIGHTING);
120 | 		for (int i=0; i<m_skel->fGraph().verts.size(); i++) {
121 | 			string jointName = m_skel->jointIdxToName[i];
122 | 			string prevJointName = m_skel->jointIdxToName[m_skel->fPrev()[i]];
123 | 			EigV3f v = Pinoc2EigV3f(m_po.embedding[i]);
124 | 			
125 | 			
126 | 			glPushMatrix();
127 | 			glTranslated(v[0], v[1], v[2]);
128 | 			glColor4f(1.0, 0, 1.0, 1.0);
129 | 			glutSolidSphere(0.01, 10, 10);
130 | 			glPopMatrix();
131 | 			
132 | 			//bone
133 | 			if (jointName != "shoulders") {
134 | 				EigV3f prev_v = Pinoc2EigV3f(m_po.embedding[m_skel->fPrev()[i]]);
135 | 				glBegin(GL_LINES);
136 | 				glColor4f(1.0, 1.0, 0.0, 1.0);
137 | 				glVertex3fv(&(v[0]));
138 | 				glVertex3fv(&(prev_v[0]));
139 | 				glEnd();
140 | 			}
141 | 		}	
142 | 		glEnable(GL_LIGHTING);
143 | 		glEnable(GL_DEPTH_TEST);
144 | 	}
145 | }
146 | 
147 | void KinectSkeletonRigger::drawMesh(bool show_original) { 
148 | 	if (m_display_mesh.vertices.size() > 0) {
149 | 		glColor4f(0.55, 0.25, 0.25, 1.0);
150 | 		drawMesh(m_display_mesh, true, Vector3(0,0,0)); 
151 | 	}
152 | 	if(show_original && m_m.vertices.size() > 0) {
153 | 		glColor4f(0.55, 0.55, 0.55, 0.25);
154 | 		drawMesh(m_m, true, Vector3(0,0,0)); 
155 | 	}
156 | }
157 | 
158 | void KinectSkeletonRigger::drawMesh(const Mesh &m, bool flatShading, Vector3 trans)
159 | {
160 | 	int i;
161 | 	Vector3 normal;
162 | 	
163 | 	glBegin(GL_TRIANGLES);
164 | 	for(i = 0; i < (int)m.edges.size(); ++i) {
165 | 		int v = m.edges[i].vertex;
166 | 		const Vector3 &p = m.vertices[v].pos;
167 | 		
168 | 		if(!flatShading) {
169 | 			normal = m.vertices[v].normal;
170 | 			glNormal3d(normal[0], normal[1], normal[2]);
171 | 		}
172 | 		else if(i % 3 == 0) {
173 | 			const Vector3 &p2 = m.vertices[m.edges[i + 1].vertex].pos;
174 | 			const Vector3 &p3 = m.vertices[m.edges[i + 2].vertex].pos;
175 | 			
176 | 			normal = ((p2 - p) % (p3 - p)).normalize();
177 | 			glNormal3d(normal[0], normal[1], normal[2]);
178 | 		}
179 | 		
180 | 		glVertex3d(p[0] + trans[0], p[1] + trans[1], p[2] + trans[2]);
181 | 	}
182 | 	glEnd();
183 | }
184 | 
185 | void KinectSkeletonRigger::LoadRig(const std::string& f) {
186 | 	if(f=="") return;
187 | 	ifstream ifs(f.c_str());
188 | 	string s_; 
189 | 	ifs >> m_meshfile;
190 | 	LoadMesh(m_meshfile);
191 | 	if(m_m.vertices.size() == 0) {
192 | 		cerr << "can't load mesh"<<endl; return;
193 | 	}
194 | 	ifs >> m_skeletonfile;
195 | 	LoadSkeletonFromText(m_skeletonfile);
196 | 	if(m_skel.get() == NULL || m_skel->fGraph().verts.size() == 0) {
197 | 		cerr << "can't load skeleton"<<endl; return;
198 | 	}
199 | 	int embedding_size,tmp;
200 | 	ifs >> embedding_size;
201 | 	m_po.embedding.resize(embedding_size);
202 | 	for (int i=0; i<embedding_size; i++) {
203 | 		ifs >> tmp >> m_po.embedding[i][0] >> m_po.embedding[i][1] >> m_po.embedding[i][2] >> tmp;
204 | 	}
205 | 	int attachment_size;
206 | 	ifs >> attachment_size;
207 | 	if(m_po.attachment == NULL) m_po.attachment = new Attachment;
208 | 	my_weights.resize(attachment_size);
209 | 	for (int i=0; i<attachment_size; i++) {
210 | 		//		my_weights[i].resize(embedding_size-1);
211 | 		for (int v=0; v<embedding_size-1; v++) {
212 | 			ifs >> my_weights[i][v];
213 | 		}
214 | 		//		cout << my_weights[i];
215 | 	}
216 | 	
217 | 	for (int v_idx = 0; v_idx < m_skel->fGraph().verts.size(); v_idx++) 
218 | 	{
219 | 		int prev_idx = m_skel->fPrev()[v_idx];
220 | 		string joint_name = m_skel->jointIdxToName[v_idx];
221 | 		string prev_joint_name = m_skel->jointIdxToName[prev_idx];
222 | 		if (joint_name == "shoulders") {
223 | 			prev_joint_name = "back";
224 | 			prev_idx = m_skel->getJointForName("back");
225 | 		}
226 | 		EigV3f skel_offset = m_skel->jointV[joint_name] - m_skel->jointV[prev_joint_name];
227 | 		EigV3f embed_offest = Pinoc2EigV3f(m_po.embedding[v_idx]) - Pinoc2EigV3f(m_po.embedding[prev_idx]);
228 | 		embedding_scale[joint_name] = embed_offest.norm() / skel_offset.norm(); // / ;
229 | 		
230 | 		cout << "embedding_scale " << joint_name << ": " << embedding_scale[joint_name] << endl;		
231 | 	}
232 | }
233 | 
234 | void KinectSkeletonRigger::SaveRig(const std::string& f) {
235 | 	if (f == "") {
236 | 		return;
237 | 	}
238 | 	if (m_meshfile == "" || m_skeletonfile == "") {
239 | 		cerr << "no mesh or skeleton loaded" << endl;
240 | 		return;
241 | 	}
242 | 	if (m_po.attachment == NULL) {
243 | 		cerr << "no rigging" << endl;
244 | 		return;
245 | 	}
246 | 	ofstream ofs(f.c_str());
247 | 	ofs << m_meshfile << endl << m_skeletonfile << endl;
248 | 	
249 | 	//output embedding
250 | 	ofs << m_po.embedding.size() << endl;
251 | 	for(int i = 0; i < (int)m_po.embedding.size(); ++i) {
252 |         ofs << i << " " << m_po.embedding[i][0] << " " << m_po.embedding[i][1] <<
253 | 		" " << m_po.embedding[i][2] << " " << m_skel->fPrev()[i] << endl;
254 |     }
255 | 	
256 |     //output attachment
257 | 	ofs << m_m.vertices.size() << endl;
258 |     for(int i = 0; i < (int)m_m.vertices.size(); ++i) {
259 |         Vector<double, -1> v = my_weights[i];
260 |         for(int j = 0; j < v.size(); ++j) {
261 |             double d = floor(0.5 + v[j] * 10000.) / 10000.;
262 |             ofs << d << " ";
263 |         }
264 |     }
265 | 	ofs.close();
266 | }
267 | 


--------------------------------------------------------------------------------
/Pinocchio/quaddisttree.h:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #ifndef QUADDISTTREE_H
 20 | #define QUADDISTTREE_H
 21 | 
 22 | #include "hashutils.h"
 23 | #include "dtree.h"
 24 | #include "multilinear.h"
 25 | #include "intersector.h"
 26 | #include "pointprojector.h"
 27 | #include <numeric>
 28 | #include <map>
 29 | 
 30 | template<int Dim>
 31 | class DistFunction : public Multilinear<double, Dim>
 32 | {
 33 |     typedef Multilinear<double, Dim> super;
 34 |     typedef Rect<double, Dim> MyRect;
 35 | public:
 36 |     template<class Eval> void initFunc(const Eval &eval, const MyRect &rect)
 37 |     {
 38 |         for(int i = 0; i < (1 << Dim); ++i) {
 39 |             setValue(i, eval(rect.getCorner(i)));
 40 |         }
 41 |         return;
 42 |     }
 43 |     
 44 | private:
 45 | };
 46 | 
 47 | template<int Dim>
 48 | class DistData : public DistFunction<Dim>
 49 | {
 50 | public:
 51 |     typedef DistFunction<Dim> super;
 52 |     typedef DNode<DistData<Dim>, Dim> NodeType;
 53 | 
 54 |     DistData(NodeType *inNode) : node(inNode) {}
 55 | 
 56 |     void init() { }
 57 | 
 58 |     template<class Eval, template<typename Node, int IDim> class Indexer>
 59 |     void fullSplit(const Eval &eval, double tol, DRootNode<DistData<Dim>, Dim, Indexer> *rootNode, int level = 0, bool cropOutside = false)
 60 |     {
 61 |         int i;
 62 |         const Rect<double, Dim> &rect = node->getRect();
 63 |         node->initFunc(eval, rect);
 64 |         
 65 |         bool nextCropOutside = cropOutside;
 66 |         if(cropOutside && level > 0) {
 67 |             double center = eval(rect.getCenter());
 68 |             double len = rect.getSize().length() * 0.5;
 69 |             if(center > len)
 70 |                 return;
 71 |             if(center < -len)
 72 |                 nextCropOutside = false;
 73 |         }
 74 |         
 75 |         if(level == (32 / Dim))
 76 |             return;
 77 |         bool doSplit = false;
 78 |         if(node->getParent() == NULL)
 79 |             doSplit = true;
 80 |         if(!doSplit) {
 81 |             int idx[Dim + 1];
 82 |             for(i = 0; i < Dim + 1; ++i)
 83 |                 idx[i] = 0;
 84 |             Vector<double, Dim> center = rect.getCenter();
 85 |             while(idx[Dim] == 0) {
 86 |                 Vector<double, Dim> cur;
 87 |                 bool anyMid = false;
 88 |                 for(i = 0; i < Dim; ++i) {
 89 |                     switch(idx[i]) {
 90 |                         case 0: cur[i] = rect.getLo()[i]; break;
 91 |                         case 1: cur[i] = rect.getHi()[i]; break;
 92 |                         case 2: cur[i] = center[i]; anyMid = true; break;
 93 |                     }
 94 |                 }
 95 |                 if(anyMid && fabs(evaluate(cur) - eval(cur)) > tol) {
 96 |                     doSplit = true;
 97 |                     break;
 98 |                 }
 99 |                 for(i = 0; i < Dim + 1; ++i) {
100 |                     if(idx[i] != 2) {
101 |                         idx[i] += 1;
102 |                         for(--i; i >= 0; --i)
103 |                             idx[i] = 0;
104 |                         break;
105 |                     }
106 |                 }
107 |             }
108 |         }
109 |         if(!doSplit)
110 |             return;
111 |         rootNode->split(node);
112 |         for(i = 0; i < NodeType::numChildren; ++i) {
113 |             eval.setRect(Rect<double, Dim>(rect.getCorner(i)) | Rect<double, Dim>(rect.getCenter()));
114 |             node->getChild(i)->fullSplit(eval, tol, rootNode, level + 1, nextCropOutside);
115 |         }
116 |     }
117 | 
118 |     template<class Real> Real evaluate(const Vector<Real, Dim> &v)
119 |     {
120 |         if(node->getChild(0) == NULL) {
121 |             return super::evaluate((v - node->getRect().getLo()).apply(divides<Real>(),
122 |                                                                        node->getRect().getSize()));
123 |         }
124 |         Vector<Real, Dim> center = node->getRect().getCenter();
125 |         int idx = 0;
126 |         for(int i = 0; i < Dim; ++i)
127 |             if(v[i] > center[i])
128 |                 idx += (1 << i);
129 |         return node->getChild(idx)->evaluate(v);
130 |     }
131 | 
132 |     template<class Real> Real integrate(Rect<Real, Dim> r)
133 |     {
134 |         r &= Rect<Real, Dim>(node->getRect());
135 |         if(r.isEmpty())
136 |             return Real();
137 |         if(node->getChild(0) == NULL) {
138 |             Vector<Real, Dim> corner = node->getRect().getLo(), size = node->getRect().getSize();
139 |             Rect<Real, Dim> adjRect((r.getLo() - corner).apply(divides<Real>(), size),
140 |                                     (r.getHi() - corner).apply(divides<Real>(), size));
141 |             return Real(node->getRect().getContent()) * super::integrate(adjRect);
142 |         }
143 |         Real out = Real();
144 |         for(int i = 0; i < NodeType::numChildren; ++i)
145 |             out += node->getChild(i)->integrate(r);
146 |         return out;
147 |     }
148 | 
149 | private:
150 |     NodeType *node;
151 | };
152 | 
153 | typedef DistData<3>::NodeType OctTreeNode;
154 | typedef DRootNode<DistData<3>, 3> OctTreeRoot;
155 | 
156 | template<class RootNode = OctTreeRoot> class OctTreeMaker 
157 | {
158 | public:
159 |     static RootNode *make(const ObjectProjector<3, Tri3Object> &proj, const Mesh &m, double tol)
160 |     {
161 |         DistObjEval eval(proj, m);
162 |         RootNode *out = new RootNode();
163 | 
164 |         out->fullSplit(eval, tol, out, 0, true);
165 |         out->preprocessIndex();
166 | 
167 |         return out;
168 |     }
169 | 
170 |     static RootNode *make(const ObjectProjector<3, Vec3Object> &proj, double tol, const RootNode *dTree = NULL)
171 |     {
172 |         PointObjDistEval eval(proj, dTree);
173 |         RootNode *out = new RootNode();
174 | 
175 |         out->fullSplit(eval, tol, out);
176 |         out->preprocessIndex();
177 | 
178 |         return out;
179 |     }
180 | 
181 | private:
182 |     class DistObjEval
183 |     {
184 |     public:
185 |         DistObjEval(const ObjectProjector<3, Tri3Object> &inProj, const Mesh &m) : proj(inProj), mint(m, Vector3(1, 0, 0))
186 |         {
187 |             level = 0;
188 |             rects[0] = Rect3(Vector3(), Vector3(1.));
189 |             inside[0] = 0;
190 |         }
191 | 
192 |         double operator()(const Vector3 &vec) const
193 |         {
194 |             unsigned int cur = ROUND(vec[0] * 1023.) + 1024 * (ROUND(vec[1] * 1023.) + 1024 * ROUND(vec[2] * 1023.));
195 |             unsigned int sz = cache.size();
196 |             double &d = cache[cur];
197 |             if(sz == cache.size())
198 |                 return d;
199 |             return d = compute(vec);
200 |         }
201 | 
202 |         void setRect(const Rect3 &r) const
203 |         {
204 |             while(!(rects[level].contains(r.getCenter()))) --level;
205 | 
206 |             inside[level + 1] = inside[level];
207 |             ++level;
208 |             if(!inside[level]) {
209 |                 double d = (*this)(r.getCenter());
210 |                 double diag2 = r.getSize().length() * 0.5;
211 |             
212 |                 if(d >= diag2)
213 |                     inside[level] = 1;
214 |                 else if(d <= -diag2)
215 |                     inside[level] = -1;
216 |             }
217 |                        
218 |             rects[level] = r;
219 |         }
220 | 
221 |     private:
222 |         double compute(const Vector3 &vec) const
223 |         {
224 |             int i, ins = inside[level];
225 |             if(!ins) {
226 |                 ins = 1;
227 |                 vector<Vector3> isecs = mint.intersect(vec);
228 |                 for(i = 0; i < (int)isecs.size(); ++i) {
229 |                     if(isecs[i][0] > vec[0])
230 |                         ins = -ins;
231 |                 }
232 |             }
233 |             
234 |             return (vec - proj.project(vec)).length() * ins;
235 |         }
236 |         
237 |         mutable map<unsigned int, double> cache;
238 |         const ObjectProjector<3, Tri3Object> &proj;
239 |         Intersector mint;
240 |         mutable Rect3 rects[11];
241 |         mutable int inside[11];
242 |         mutable int level; //essentially index of last rect
243 |     };
244 |     
245 |     class PointObjDistEval
246 |     {
247 |     public:
248 |         PointObjDistEval(const ObjectProjector<3, Vec3Object> &inProj, const RootNode *inDTree) : proj(inProj), dTree(inDTree) {}
249 | 
250 |         double operator()(const Vector3 &vec) const
251 |         {
252 |             unsigned int cur = ROUND(vec[0] * 1023.) + 1024 * (ROUND(vec[1] * 1023.) + 1024 * ROUND(vec[2] * 1023.));
253 |             unsigned int sz = cache.size();
254 |             double &d = cache[cur];
255 |             if(sz == cache.size())
256 |                 return d;
257 |             return d = (vec - proj.project(vec)).length();
258 |         }
259 | 
260 |         void setRect(const Rect3 &r) const { }
261 | 
262 |     private:
263 |         mutable hash_map<unsigned int, double> cache;
264 |         const ObjectProjector<3, Vec3Object> &proj;
265 |         const RootNode *dTree;
266 |     };
267 | };
268 | #endif
269 | 


--------------------------------------------------------------------------------
/Pinocchio/matrix.h:
--------------------------------------------------------------------------------
  1 | /*  This file is part of the Pinocchio automatic rigging library.
  2 |     Copyright (C) 2007 Ilya Baran (ibaran@mit.edu)
  3 | 
  4 |     This library is free software; you can redistribute it and/or
  5 |     modify it under the terms of the GNU Lesser General Public
  6 |     License as published by the Free Software Foundation; either
  7 |     version 2.1 of the License, or (at your option) any later version.
  8 | 
  9 |     This library is distributed in the hope that it will be useful,
 10 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 12 |     Lesser General Public License for more details.
 13 | 
 14 |     You should have received a copy of the GNU Lesser General Public
 15 |     License along with this library; if not, write to the Free Software
 16 |     Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 17 | */
 18 | 
 19 | #ifndef MATRIX_H_INCLUDED
 20 | #define MATRIX_H_INCLUDED
 21 | 
 22 | #include <vector>
 23 | #include <algorithm>
 24 | #include <iostream>
 25 | #include <cassert>
 26 | 
 27 | #include "mathutils.h"
 28 | 
 29 | template<class Real>
 30 | class Vectorn : public vector<Real>
 31 | {
 32 | public:
 33 |     typedef Vectorn<Real> Self;
 34 |     typedef vector<Real> super;
 35 | 
 36 |     Vectorn() {}
 37 |     Vectorn(const Vectorn &other) : super(other) {}
 38 |     Vectorn(int sz, const Real &init = Real()) : super(sz, init) {}
 39 |     
 40 |     template<class R> Vectorn(const vector<R> &other) : super(other.begin(), other.end()) {}
 41 |     template<class Iter> Vectorn(const Iter &from, const Iter &to) : super(from, to) {}
 42 | 
 43 |     using super::size;
 44 | 
 45 |     //basic recursive functions
 46 |     template<class F> Vectorn<typename F::result_type> apply(const F &func) const
 47 |     {
 48 |         Vectorn<typename F::result_type> out(size());
 49 |         for(int i = 0; i < (int)size(); ++i)
 50 |             out[i] = func((*this)[i]);
 51 |         return out;
 52 |     }
 53 | 
 54 |     template<class F> Vectorn<typename F::result_type> apply(const F &func, const Self &other) const
 55 |     {
 56 |         assert(size() == other.size());
 57 |         Vectorn<typename F::result_type> out(size());
 58 |         for(int i = 0; i < (int)size(); ++i)
 59 |             out[i] = func((*this)[i], other[i]);
 60 |         return out;
 61 |     }
 62 | 
 63 | 
 64 |     template<class Op, class Accum>
 65 |     typename Accum::result_type accumulate(const Op &op, const Accum &accum) const
 66 |     {
 67 |         if(size() == 0)
 68 |             return typename Accum::result_type();
 69 |         typename Accum::result_type out(op((*this)[0]));
 70 |         for(int i = 1; i < (int)size(); ++i)
 71 |             out = accum(out, op((*this)[i]));
 72 |         return out;
 73 |     }
 74 | 
 75 |     template<class Op, class Accum>
 76 |     typename Accum::result_type accumulate(const Op &op, const Accum &accum, const Self &other) const
 77 |     {
 78 |         if(size() == 0)
 79 |             return typename Accum::result_type();
 80 |         assert(size() == other.size());
 81 |         typename Accum::result_type out(op((*this)[0], other[0]));
 82 |         for(int i = 1; i < (int)size(); ++i)
 83 |             out = accum(out, op((*this)[i], other[i]));
 84 |         return out;
 85 |     }
 86 | 
 87 |     Real operator*(const Self &other) const { return accumulate(multiplies<Real>(), plus<Real>(), other); }
 88 |     Self operator+(const Self &other) const { return apply(plus<Real>(), other); }
 89 |     Self operator-(const Self &other) const { return apply(minus<Real>(), other); }
 90 |     Self operator*(const Real &scalar) const { return apply(bind2nd(multiplies<Real>(), scalar)); }
 91 |     Self operator/(const Real &scalar) const { return apply(bind2nd(divides<Real>(), scalar)); }
 92 |     Self operator-() const { return apply(negate<Real>()); }
 93 |     bool operator==(const Self &other) const { return accumulate(equal_to<Real>(), logical_and<Real>(), other); }
 94 | 
 95 | #define OPAS(op, typ) Self &operator op##=(const typ &x) { (*this) = (*this) op x; return *this; }
 96 |     OPAS(+, Self)
 97 |     OPAS(-, Self)
 98 |     OPAS(*, Real)
 99 |     OPAS(/, Real)
100 | #undef OPAS
101 | 
102 |     Real lengthsq() const { return (*this) * (*this); }
103 |     Real length() const { return sqrt(lengthsq()); }
104 | 
105 |     Real sum() const { return accumulate(ident<Real>(), plus<Real>()); }
106 | 
107 |     Self normalize() const { return (*this) / length(); }
108 | 
109 | };
110 | 
111 | template<class Real>
112 | inline ostream &operator<<(ostream &os, const Vectorn<Real> &v)
113 | {
114 |     os << "[";
115 |     for(int i = 0; i < (int)v.size(); ++i) {
116 |         os << v[i];
117 |         if(i < (int)v.size() - 1)
118 |             os << " ";
119 |     }
120 |     os << "]";
121 |     return os;
122 | }
123 | 
124 | template<class Real>
125 | class Matrixn
126 | {
127 | public:
128 |     typedef Matrixn<Real> Self;
129 |     typedef Vectorn<Real> Vec;
130 | 
131 |     Matrixn() {}
132 |     Matrixn(int rows, int cols, const Real &init = Real()) : m(rows, Vec(cols, init)) {}
133 |     static Self identity(int sz, const Real &diag = Real(1.))
134 |     {
135 |         Self out(sz, sz);
136 |         for(int i = 0; i < sz; ++i)
137 |             out[i][i] = diag;
138 |         return out;
139 |     }
140 | 
141 |     static Self identity(const Vec &diag)
142 |     {
143 |         Self out(diag.size(), diag.size());
144 |         for(int i = 0; i < (int)diag.size(); ++i)
145 |             out[i][i] = diag[i];
146 |         return out;
147 |     }
148 | 
149 |     const Real &operator()(int row, int col) const { return m[row][col]; }
150 |     Real &operator()(int row, int col) { return m[row][col]; }
151 | 
152 |     const Vec &operator[](int row) const { return m[row]; }
153 |     Vec &operator[](int row) { return m[row]; }
154 | 
155 |     int getRows() const { return m.size(); }
156 |     int getCols() const { return m[0].size(); }
157 | 
158 |     Real sum() const { return m.sum().sum(); }
159 | 
160 |     Self operator+(const Self &oth) const { return Self(m + oth.m); }
161 |     Self operator-(const Self &oth) const { return Self(m - oth.m); }
162 |     Self operator*(double oth) const { return Self(m.apply(bind2nd(myMult<Vec, Real, Vec>(), oth))); }
163 |     Self operator/(double oth) const { return Self(m.apply(bind2nd(myDiv<Vec, Real, Vec>(), oth))); }
164 |     Self operator-() const { return Self(-m); }
165 | 
166 |     Vec operator*(const Vec &oth) const { return m.apply(bind2nd(myMult<Vec, Vec, Real>(), oth)); }
167 | 
168 |     Self operator*(const Self &oth) const
169 |     {
170 |         assert(getCols() == oth.getRows());
171 |         Self out(getRows(), oth.getCols());
172 |         for(int i = 0; i < getRows(); ++i) for(int j = 0; j < oth.getCols(); ++j) {
173 |             out[i][j] = m[i] * oth.getColumn(j);
174 |         }
175 |         return out;
176 |     }
177 | 
178 |     Self operator~() const //transpose
179 |     {
180 |         Self out(getCols(), getRows());
181 |         for(int i = 0; i < out.getRows(); ++i)
182 |             out[i] = getColumn(i);
183 |         return out;
184 |     }
185 | 
186 |     Self operator!() const //invert
187 |     {
188 |         assert(getRows() == getCols());
189 | 
190 |         Self out = identity(m.size());
191 |         Self tmp = *this;
192 |         
193 |         int i, j;
194 |         for(i = 0; i < getRows(); ++i) {
195 |             //find pivot
196 |             int pivot = -1;
197 |             double biggestSoFar = -1.;
198 |             for(j = i; j < getRows(); ++j) {
199 |                 double cur = myabs(tmp.m[j][i]);
200 |                 if(cur > biggestSoFar) {
201 |                     biggestSoFar = cur;
202 |                     pivot = j;
203 |                 }
204 |             }
205 |             
206 |             assert(biggestSoFar > 1e-10); //nonsingular
207 |             if(biggestSoFar <= 1e-10)
208 |                 return out; //whatever
209 |             
210 |             //move pivot to the right place
211 |             std::swap(tmp.m[i], tmp.m[pivot]);
212 |             std::swap(out.m[i], out.m[pivot]);
213 |             
214 |             double cur = tmp.m[i][i];
215 |             
216 |             //divide row by element
217 |             tmp.m[i] /= cur;
218 |             out.m[i] /= cur;
219 |             
220 |             //now subtract something times this row from other rows
221 |             for(j = 0; j < getRows(); ++j) {
222 |                 if(j == i)
223 |                     continue;
224 |                 
225 |                 double cur = tmp.m[j][i];
226 |                 tmp.m[j] -= tmp.m[i] * cur;
227 |                 out.m[j] -= out.m[i] * cur;
228 |             }
229 |         }
230 |         
231 |         return out;
232 |     }
233 | 
234 |     Vec getColumn(int idx) const
235 |     {
236 |         assert(idx >= 0 && idx < getCols());
237 |         Vec out(getRows());
238 |         for(int i = 0; i < getRows(); ++i)
239 |             out[i] = m[i][idx];
240 |         return out;
241 |     }
242 | 
243 |     Real det() const
244 |     {
245 |         Real out = 1.;
246 | 
247 |         Self tmp = *this;
248 |         int i, j;
249 |         for(i = 0; i < (int)m.size(); ++i) {
250 |             //find pivot
251 |             int pivot = -1;
252 |             double biggestSoFar = -1.;
253 |             for(j = i; j < (int)m.size(); ++j) {
254 |                 double cur = myabs(tmp.m[j][i]);
255 |                 if(cur > biggestSoFar) {
256 |                     biggestSoFar = cur;
257 |                     pivot = j;
258 |                 }
259 |             }
260 | 
261 |             if(biggestSoFar <= 1e-10)
262 |                 return Real(0.); //singular
263 | 
264 |             //move pivot to the right place
265 |             std::swap(tmp.m[i], tmp.m[pivot]);
266 |             if(pivot != i)
267 |                 out = -out;
268 | 
269 |             Real cur = tmp.m[i][i];
270 | 
271 |             //multiply determinant by element
272 |             out *= cur;
273 | 
274 |             //now subtract something times this row from other rows
275 |             for(j = i + 1; j < (int)m.size(); ++j) {
276 |                 Real fact = tmp.m[j][i] / cur;
277 |                 for(int k = i + 1; k < (int)m.size(); ++k) {
278 |                     tmp.m[j][k] -= fact * tmp.m[i][k];
279 |                 }
280 |             }
281 |         }
282 | 
283 |         return out;
284 |     }
285 | 
286 | private:
287 |     static double myabs(double d) { return fabs(d); }
288 | 
289 |     Matrixn(const Vectorn<Vectorn<Real> > &inM) : m(inM) {}
290 |     Vectorn<Vectorn<Real> > m;
291 | };
292 | 
293 | Vectorn<double> getEigensystem(Matrixn<double> m, Matrixn<double> *eigenvectors = NULL);
294 | Vectorn<double> getSVD(const Matrixn<double> &m, Matrixn<double> &u, Matrixn<double> &v);
295 | 
296 | template<class Real>
297 | inline ostream &operator<<(ostream &os, const Matrixn<Real> &m)
298 | {
299 |     os << "[";
300 |     for(int i = 0; i < (int)m.getRows(); ++i) {
301 |         if(i > 0)
302 |             os << " ";
303 |         os << m[i];
304 |         if(i + 1 < (int)m.getRows())
305 |             os << endl;
306 |     }
307 |     os << "]" << endl;
308 |     return os;
309 | }
310 | 
311 | #endif //MATRIX_H_INCLUDED
312 | 


--------------------------------------------------------------------------------