add ik

6_ik/CMakeLists.txt

+
+cmake_minimum_required(VERSION 3.1)
+project(6_ik)
+
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/../cmake)
+set(CMAKE_CXX_FLAGS "-Wall")
+
+# libigl
+option(LIBIGL_USE_STATIC_LIBRARY "Use libigl as static library" OFF)
+option(LIBIGL_WITH_ANTTWEAKBAR       "Use AntTweakBar"    OFF)
+option(LIBIGL_WITH_CGAL              "Use CGAL"           OFF)
+option(LIBIGL_WITH_COMISO            "Use CoMiso"         OFF)
+option(LIBIGL_WITH_CORK              "Use Cork"           OFF)
+option(LIBIGL_WITH_EMBREE            "Use Embree"         OFF)
+option(LIBIGL_WITH_LIM               "Use LIM"            OFF)
+option(LIBIGL_WITH_MATLAB            "Use Matlab"         OFF)
+option(LIBIGL_WITH_MOSEK             "Use MOSEK"          OFF)
+option(LIBIGL_WITH_OPENGL            "Use OpenGL"         ON)
+option(LIBIGL_WITH_OPENGL_GLFW       "Use GLFW"           ON)
+option(LIBIGL_WITH_OPENGL_GLFW_IMGUI "Use ImGui"          ON)
+option(LIBIGL_WITH_PNG               "Use PNG"            OFF)
+option(LIBIGL_WITH_PYTHON            "Use Python"         OFF)
+option(LIBIGL_WITH_TETGEN            "Use Tetgen"         OFF)
+option(LIBIGL_WITH_TRIANGLE          "Use Triangle"       OFF)
+option(LIBIGL_WITH_VIEWER            "Use OpenGL viewer"  ON)
+option(LIBIGL_WITH_XML               "Use XML"            OFF)
+
+if (NOT LIBIGL_FOUND)
+    find_package(LIBIGL REQUIRED QUIET)
+endif()
+
+# Add default project files
+file(GLOB LIBFILES ${PROJECT_SOURCE_DIR}/../include/*.*)
+source_group("Library Files" FILES ${LIBFILES})
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../include)
+
+# Add your project files
+file(GLOB SRCFILES *.cpp)
+file(GLOB HFILES *.h)
+
+add_definitions(-DIGL_VIEWER_VIEWER_QUIET)
+add_executable(${PROJECT_NAME} ${SRCFILES} ${HFILES} ${LIBFILES} )
+target_link_libraries(${PROJECT_NAME} igl::core igl::opengl_glfw igl::opengl_glfw_imgui)
\ No newline at end of file

6_ik/IkSim.cpp

+#include "IkSim.h"
+
+void Skeleton::init() {
+	m_bones.clear();
+
+	// left arm
+	Bone leftArm(Eigen::Vector3d(-1.2, 3, 0));
+	leftArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(-0.5, -1.7, -0.8));
+	leftArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -1.4, 1.4));
+
+	// right arm
+	Bone rightArm(Eigen::Vector3d(1.2, 3, 0));
+	rightArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0.5, -1.7, -0.8));
+	rightArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -1.4, 1.4));
+
+	// left leg
+	Bone leftLeg(Eigen::Vector3d(-0.7, 0, 0));
+	leftLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, 0.2));
+	leftLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, -0.4));
+	leftLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 1.2));
+
+	// right leg
+	Bone rightLeg(Eigen::Vector3d(0.7, 0, 0));
+	rightLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, 0.2));
+	rightLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, -0.4));
+	rightLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 1.2));
+
+	// head
+	Bone head(Eigen::Vector3d(0, 3, 0));
+	head.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0.7, 0));
+
+	m_bones.emplace_back(leftArm);
+	m_bones.emplace_back(rightArm);
+	m_bones.emplace_back(leftLeg);
+	m_bones.emplace_back(rightLeg);
+	m_bones.emplace_back(head);
+}
+
+std::vector<Eigen::Vector3d> IkSim::updateMotion() {
+	std::vector<Eigen::Vector3d> targetPos;
+
+	Eigen::Vector3d footPos1 = Eigen::Vector3d(0, 0, 3);
+	Eigen::Vector3d footPos2 = Eigen::Vector3d(0, 0, -4);
+
+	for (int i = 0; i < m_skeleton.m_bones.size(); i++) {
+		Bone& bone = m_skeleton.m_bones[i];
+		Eigen::Vector3d endPos = m_bone_init_end[i];
+
+		//todo: update endPos based on m_step
+
+		targetPos.push_back(endPos);
+	}
+
+	return targetPos;
+}
+
+bool IkSim::advance() {
+	// advance step
+	m_step++;
+
+	int iterNum = 100;
+	float alpha = 0.01;
+	std::vector<Eigen::Vector3d> targetPos = updateMotion();
+
+
+	//todo: update skeleton position use inverse kinematics
+
+	return false;
+}
+
+Eigen::MatrixXd Bone::computeJacobian(Eigen::Vector3d endPos) {
+	Eigen::MatrixXd J(3, m_num_joints * 3);
+
+	// todo: compute the Jacobian matrix
+
+	return J;
+}
+
+Eigen::Vector3d Bone::forwardKinematics(int joint_index) {
+	if (joint_index < -1 || joint_index >= m_num_joints) {
+		printf("joint index out of range\n");
+		exit(-1);
+	}
+	//todo: compute the forward kinematics of the joint in the local coordinate system of skeleton
+	//joint_index = -1 means the root position
+	//jointindex = 0 means the first joint
+	//joint_index = m_num_joints - 1 means the end effector position
+	Eigen::Vector3d pos = m_root;
+
+	return pos;
+}
\ No newline at end of file

6_ik/IkSim.h

+#include "Simulation.h"
+#include <time.h>
+using namespace std;
+
+inline float getRandomFloat(float min, float max) {
+	return min + static_cast<float>(rand()) / (static_cast<float>(RAND_MAX / (max - min)));
+}
+
+struct Joint {
+	Eigen::Vector3d angles; // euler angles in degrees
+	Eigen::Vector3d extent; // length and direction of the bone
+	Eigen::Matrix3d toRotation() {
+		Eigen::Matrix3d rotation;
+		rotation = Eigen::AngleAxisd(angles[2] * M_PI / 180, Eigen::Vector3d::UnitZ())
+			* Eigen::AngleAxisd(angles[1] * M_PI / 180, Eigen::Vector3d::UnitY())
+            * Eigen::AngleAxisd(angles[0] * M_PI / 180, Eigen::Vector3d::UnitX());
+		return rotation;
+	}
+    Joint() : angles(Eigen::Vector3d(0, 0, 0)), extent(Eigen::Vector3d(1, 0, 0)) {} // default constructor
+	Joint(Eigen::Vector3d angles, Eigen::Vector3d extent) : angles(angles), extent(extent) {}
+	~Joint() {}
+};
+
+struct Bone {
+    Bone(): m_root(Eigen::Vector3d(0, 0, 0)) {}
+	Bone(Eigen::Vector3d root) : m_root(root) {}
+    ~Bone() {}
+    void init(int num_joints) {
+		srand(time(NULL));
+        m_num_joints = num_joints;
+        m_joints.clear();
+        m_joints.reserve(num_joints);
+        for (int i = 0; i < num_joints; ++i) {
+            Eigen::Vector3d angles = Eigen::Vector3d(0, 0, 90 - (i % 2) * 180);
+            if (i == 0)
+				angles = Eigen::Vector3d(0, 0, 45);
+			else
+				angles = Eigen::Vector3d(0, 0, -45);
+            Eigen::Vector3d extent = Eigen::Vector3d(1, 0, 0);
+            m_joints.emplace_back(angles, extent);
+        }
+    }
+    
+    int m_num_joints = 0;  // number of joints in the bone
+	Eigen::Vector3d m_root; // root position of the bone
+    std::vector<Joint> m_joints;  // joints of the bone
+
+    Eigen::Vector3d forwardKinematics(int joint_index);
+
+    Eigen::MatrixXd computeJacobian(Eigen::Vector3d endPos);
+
+	void addJoint(Eigen::Vector3d angles, Eigen::Vector3d extent) {
+		m_joints.emplace_back(angles, extent);
+		m_num_joints++;
+	}
+};
+
+struct Skeleton {
+    Eigen::Vector3d base_pos = Eigen::Vector3d::Zero();
+	std::vector<Bone> m_bones; // bones of the skeleton
+    Skeleton(){}
+    ~Skeleton(){}
+    void init();
+	void addBone(Bone& bone) {
+        m_bones.emplace_back(bone);
+	}
+};
+
+class IkSim : public Simulation {
+public:
+    IkSim() : Simulation() {
+        init();
+    }
+
+	virtual void init() override {
+        m_skeleton.base_pos = Eigen::Vector3d(0, 0, 0); // Initialize root position
+
+        m_skeleton.init(); // Initialize skeleton with 2 joints
+
+        std::string path = "cube.off";
+        m_objects.clear();
+        for (int i = 0; i < m_skeleton.m_bones.size(); i++) {
+            Bone& bone = m_skeleton.m_bones[i];
+            for (int j = 0; j < bone.m_num_joints; j++) {
+                m_objects.emplace_back(path);
+                m_objects.back().setScale(bone.m_joints[j].extent.norm() / 2, 0.2, 0.2);
+            }
+        }
+
+        m_objects.emplace_back(path);
+        m_objects.back().setScale(1, 1.5, 0.5);
+        m_objects.back().setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 1.5, 0));
+
+        m_objects.emplace_back("sphere.off");
+        m_objects.back().setScale(0.04);
+        m_objects.back().setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 4, 0));
+
+		m_dt = 5e-2;
+
+		reset();
+	}
+
+    void initEndVector() {
+        m_bone_init_end.clear();
+		for (int i = 0; i < m_skeleton.m_bones.size(); i++) {
+			Bone& bone = m_skeleton.m_bones[i];
+			Eigen::Vector3d endPos = bone.forwardKinematics(bone.m_num_joints - 1);
+			m_bone_init_end.push_back(endPos);
+		}
+    }
+
+    virtual void resetMembers() override {
+        m_skeleton.base_pos = Eigen::Vector3d(0, 0, 0); // Initialize root position
+
+        m_skeleton.init(); // Initialize skeleton with 2 joints
+
+        initEndVector(); // Initialize end positions of bones
+    }
+
+    std::vector<Eigen::Vector3d> updateMotion();
+
+    void updateSkeletonPosition() {
+
+        int rigidIndex = 0;
+        for (int i = 0; i < m_skeleton.m_bones.size(); i++) {
+            Bone& bone = m_skeleton.m_bones[i];
+
+            Eigen::Vector3d pos = m_skeleton.base_pos + bone.m_root;
+            Eigen::Matrix3d rotate = Eigen::Matrix3d::Identity();
+			
+            for (int j = 0; j < bone.m_num_joints; j++) {
+                RigidObject& o = m_objects[rigidIndex];
+                Joint& b = bone.m_joints[j];
+
+                Eigen::Matrix3d jointRotate = bone.m_joints[j].toRotation();
+                rotate = jointRotate * rotate;
+                Eigen::Vector3d extent = rotate * b.extent;
+                o.setPosition(pos + extent / 2);
+
+                Eigen::Vector3d unit = Eigen::Vector3d(1, 0, 0);
+                Eigen::Vector3d normalExtent = b.extent.normalized();
+                Eigen::Vector3d axis = unit.cross(normalExtent);
+                double angle = acos(unit.dot(normalExtent));
+
+                Eigen::Matrix3d extentRotate = Eigen::Matrix3d::Identity();
+                if (axis.norm() > 1e-6) {
+                    axis.normalize();
+                    extentRotate = Eigen::AngleAxisd(angle, axis).toRotationMatrix();
+                }
+                o.setRotation(rotate * extentRotate);
+
+                pos += extent;
+
+                rigidIndex++;
+            }
+        }
+        
+        m_objects[rigidIndex].setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 1.5, 0));
+
+        m_objects[rigidIndex + 1].setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 4, 0));
+    }
+
+    virtual void updateRenderGeometry() override {
+		updateSkeletonPosition();
+        for (size_t i = 0; i < m_objects.size(); i++) {
+            RigidObject &o = m_objects[i];
+            if (o.getID() < 0) {    // negative id means newly created object, reverse memory for it
+                m_renderVs.emplace_back();
+                m_renderFs.emplace_back();
+            }
+
+            m_objects[i].getMesh(m_renderVs[i], m_renderFs[i]);
+        }
+    }
+
+	virtual bool advance() override;
+
+    virtual void renderRenderGeometry(igl::opengl::glfw::Viewer &viewer) override {
+        for (size_t i = 0; i < m_objects.size(); i++) {
+            RigidObject &o = m_objects[i];
+            if (o.getID() < 0) {
+                int new_id = 0;
+                if (i > 0) {
+                    new_id = viewer.append_mesh();
+                    o.setID(new_id);
+                } else {
+                    o.setID(new_id);
+                }
+
+                size_t meshIndex = viewer.mesh_index(o.getID());
+                viewer.data_list[meshIndex].set_face_based(true);
+                viewer.data_list[meshIndex].point_size = 2.0f;
+                viewer.data_list[meshIndex].clear();
+            }
+            size_t meshIndex = viewer.mesh_index(o.getID());
+
+            viewer.data_list[meshIndex].set_mesh(m_renderVs[i], m_renderFs[i]);
+            viewer.data_list[meshIndex].compute_normals();
+
+            Eigen::MatrixXd color;
+            o.getColors(color);
+            viewer.data_list[meshIndex].set_colors(color);
+        }
+    }
+
+private:
+    Skeleton m_skeleton;
+    
+	std::vector<Eigen::Vector3d> m_bone_init_end; // initial end positions of bones
+    std::vector<Eigen::MatrixXd> m_renderVs;  // vertex positions for rendering
+    std::vector<Eigen::MatrixXi> m_renderFs;  // face indices for rendering
+};
\ No newline at end of file

6_ik/main.cpp

+#include "IkSim.h"
+#include "Gui.h"
+
+/*
+ * GUI for the earth simulation.
+ */
+class IkGui : public Gui {
+public:
+    // simulation parameters
+    float m_dt = 1e-2;
+    float m_radius = 10.0;
+
+    IkSim* p_IkSim = NULL;
+
+    IkGui() {
+        // create a new Earth simulation, set it in the GUI, and start the GUI
+        p_IkSim = new IkSim();
+        setSimulation(p_IkSim);
+
+        start();
+    }
+
+    virtual void updateSimulationParameters() override {
+        // change all parameters of the simulation to the values that are set in the GUI
+        p_IkSim->setTimestep(m_dt);
+    }
+
+
+    virtual void drawSimulationParameterMenu() override {
+        ImGui::InputFloat("dt", &m_dt, 0, 0);
+    }
+};
+
+int main(int argc, char* argv[]) {
+    // create a new instance of the GUI for the Earth simulation
+    new IkGui();
+
+    return 0;
+}
\ No newline at end of file

CMakeLists.txt

@@ -36,4 +36,5 @@ add_subdirectory(2-2_pendulum)
 add_subdirectory(3-1_spinning)
 add_subdirectory(3-2_collision)
 add_subdirectory(4_cloth)
-add_subdirectory(5_fluid)
+add_subdirectory(5_fluid)
\ No newline at end of file
+add_subdirectory(6_ik)
\ No newline at end of file

include/BaseObject.cpp

@@ -60,7 +60,11 @@ void BaseObject::recomputeCOM() {
 	m_mesh.V = m_mesh.V.rowwise() - COM.transpose();
 }
 
-void BaseObject::setScale(double s) { m_scale = s; }
+void BaseObject::setScale(Eigen::Vector3d s) { m_scale = s; }
+
+void BaseObject::setScale(double s) { m_scale = Eigen::Vector3d(s, s, s); }
+
+void BaseObject::setScale(double sX, double sY, double sZ) { m_scale = Eigen::Vector3d(sX, sY, sZ); }
 
 void BaseObject::setID(int id) { m_id = id; }
 
@@ -80,7 +84,7 @@ void BaseObject::setRotation(const Eigen::Matrix3d& R) {
 
 void BaseObject::setColors(const Eigen::MatrixXd& C) { m_mesh.C = C; }
 
-double BaseObject::getScale() const { return m_scale; }
+Eigen::Vector3d BaseObject::getScale() const { return m_scale; }
 
 int BaseObject::getID() const { return m_id; }
 
@@ -93,14 +97,14 @@ Eigen::Quaterniond BaseObject::getRotation() const { return m_quat; }
 Eigen::Matrix3d BaseObject::getRotationMatrix() const { return m_rot; }
 
 Eigen::Vector3d BaseObject::getVertexPosition(int vertexIndex) const {
-	return m_mesh.V.row(vertexIndex) * m_scale *
+	return m_mesh.V.row(vertexIndex).cwiseProduct(m_scale.transpose()) *
 		getRotationMatrix().transpose() +
 		getPosition().transpose();
 }
 
 void BaseObject::getMesh(Eigen::MatrixXd& V, Eigen::MatrixXi& F) const {
 	// get mesh after rotation and translation
-	V = (m_mesh.V * m_scale * getRotationMatrix().transpose()).rowwise() +
+	V = (m_mesh.V* m_scale.asDiagonal() * getRotationMatrix().transpose()).rowwise() +
 		getPosition().transpose();
 	F = m_mesh.F;
 }

include/BaseObject.h

@@ -35,7 +35,9 @@ public:
 	void reset();
 	void recomputeCOM();
 
+	void setScale(Eigen::Vector3d s);
 	void setScale(double s);
+	void setScale(double sX, double sY, double sZ);
 	void setID(int id);
 	virtual void setType(ObjType t);
 	void setPosition(const Eigen::Vector3d& p);
@@ -43,7 +45,7 @@ public:
 	void setRotation(const Eigen::Matrix3d& R);
 	void setColors(const Eigen::MatrixXd& C);
 
-	double getScale() const;
+	Eigen::Vector3d getScale() const;
 	int getID() const;
 	ObjType getType() const;
 	Eigen::Vector3d getPosition() const;
@@ -65,7 +67,7 @@ protected:
 	Mesh m_mesh;
 	ObjType m_type;
 
-	double m_scale = 1.0;        // Scale
+	Eigen::Vector3d m_scale = Eigen::Vector3d(1.0, 1.0, 1.0);        // Scale
 	Eigen::Vector3d m_position;  // Position of the center of mass
 	Eigen::Quaterniond m_quat;   // Rotation (quaternion)
 	Eigen::Matrix3d m_rot;       // Rotation (matrix)