all: bin/main
-CXX_FLAGS := -g --std=c++17 -Wall
+CXX_FLAGS := -g --std=c++17 -Wall -DDEBUG_NODES
-bin/main: model.o material.o image.o skybox.o program.o main.o util.o
+bin/main: model.o material.o image.o skybox.o program.o ik.o main.o util.o
clang++ $(CXX_FLAGS) $^ \
-I/usr/local/include -L/usr/local/lib \
-lassimp \
+-Wall
-std=c++17
-I/usr/local/include/
+-DDEBUG_NODES
--- /dev/null
+#include "ik.hpp"
+#include "util.hpp"
+
+using namespace glm;
+
+constexpr float tolerance = 0.3;
+
+const std::vector<glm::vec3> fabrik(const glm::vec3 t,
+ const std::vector<glm::vec3> jpsIn, // joint positions
+ const std::vector<float> jds // distances between each joint
+ ) {
+ size_t N = jpsIn.size();
+ assert(N == jds.size() + 1);
+ std::vector<glm::vec3> jps = jpsIn;
+
+ float dist = distance(jps[0], t);
+ float totalLength = 0;
+ for (int i = 0; i < N - 1; i++) totalLength += jds[i];
+ if (dist > totalLength) { // target is unreachable
+ for (int i = 0; i < N - 1; i++) {
+ float r = distance(t, jps[i]);
+ float lambda = jds[i] / r;
+ jps[i + 1] = (1.f - lambda) * jps[i] + lambda * t;
+ }
+ } else { // target is reachable
+ glm::vec3 b = jps[0];
+ // distance between end effector and target
+ float diff = distance(jps[N - 1], t);
+ while (diff > tolerance) {
+ // forward reaching
+ jps[N - 1] = t; // set end effector to target
+ for (int i = N - 2; i >= 0; i--) {
+ float r = distance(jps[i + 1], jps[i]);
+ float lambda = jds[i] / r;
+ jps[i] = (1.f - lambda) * jps[i + 1] + lambda * jps[i];
+ }
+
+ // backward reaching
+ jps[0] = b; // reset root to initial pos
+ for (int i = 0; i < N - 1; i++) {
+ float r = distance(jps[i + 1], jps[i]);
+ float lambda = jds[i] / r;
+ jps[i + 1] = (1 - lambda) * jps[i] + lambda * jps[i + 1];
+ }
+
+ float newDiff = distance(jps[N - 1], t);
+ if (newDiff == diff) abort();
+ diff = newDiff;
+ }
+ }
+ return jps;
+}
+
+std::vector<Model::Node> allNodesTo(const Model::Node &from, const Model::Node &to) {
+ if (from == to) return { to };
+ assert(to.parent != nullptr);
+ auto res = allNodesTo(from, *to.parent);
+ res.push_back(to);
+ return res;
+}
+
+mat4 getAbsTrans(const Model::Node &root, const Model::Node &n) {
+ if (root.ai.mName == n.ai.mName)
+ return mat4(1); //aiMatrixToMat4(n.ai.mTransformation);
+ assert(n.parent != nullptr);
+ return getAbsTrans(root, *n.parent) * aiMatrixToMat4(n.ai.mTransformation);
+}
+
+vec3 extractPos(mat4 trans) {
+ return vec3(trans[3]);
+}
+
+glm::mat4 absoluteToModelSpace(const Model::Node &root, const Model::Node &n, mat4 m) {
+ const Model::Node *parent = &n;
+ glm::mat4 res = m;
+ std::vector<mat4> trans;
+ while (&parent->ai != &root.ai) {
+ trans.push_back(inverse(aiMatrixToMat4(parent->ai.mTransformation)));
+ parent = parent->parent;
+ }
+ while (!trans.empty()) { res = trans.back() * res; trans.pop_back(); }
+ return res;
+}
+
+// Given points u and v on a sphere, calculate the transformation matrix
+// to bring u -> v
+// from https://math.stackexchange.com/a/2765250
+mat4 getRotationToPoint(vec3 u, vec3 v, float dist) {
+ if (distance(u, v) < 0.00001) return mat4(1);
+ vec3 n = cross(u, v) / length(cross(u, v));
+ vec3 t = cross(n, u);
+ float alpha = atan2(dot(v, t), dot(v, u));
+ mat4 T = mat4(mat3(u, t, n));
+ mat4 R = rotate(mat4(1), alpha, {0, 0, 1}); // rotation in z axis
+ mat4 res = T * R * inverse(T);
+ return res;
+}
+
+
+void inverseKinematic(Model::Node &root, Model::Node &end, vec3 target) {
+ /* float s2o2 = sqrt(2.f) / 2.f; */
+ /* assert(getRotationToPoint({1, 0, 0}, {0, s2o2, s2o2}, 1) */
+ /* == mat4({0, s2o2, s2o2, 0}, { -s2o2, 1.f/2.f, -1.f/2.f, 0}, */
+ /* {-s2o2, -1.f/2.f, 1.f/2.f, 0}, { 0, 0, 0, 1})); */
+
+ std::vector<Model::Node> chain = allNodesTo(root, end);
+ assert(!chain.empty());
+
+ std::vector<vec3> positions(chain.size()); std::vector<float> distances(chain.size() - 1);
+ for (size_t i = 0; i < chain.size(); i++) {
+ mat4 absTrans = getAbsTrans(root, chain[i]);
+ positions[i] = extractPos(absTrans);
+ if (i > 0)
+ distances[i - 1] = distance(positions[i], positions[i - 1]);
+ }
+
+ /* glm::vec3 targetPos(sin(d * 10.f), cos(d * 10.f), 0); */
+ auto newPositions = fabrik(target, positions, distances);
+
+ // Rotate all the nodes so that they are in the correct positions
+ for (size_t i = 1; i < chain.size(); i++) {
+ auto node = chain[i];
+ mat4 absTrans = getAbsTrans(root, node);
+ absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform
+
+ vec3 oldRelPos = extractPos(aiMatrixToMat4(node.ai.mTransformation));
+ vec3 newRelPos = extractPos(absoluteToModelSpace(root, *node.parent, absTrans));
+
+ mat4 rot = getRotationToPoint(oldRelPos, newRelPos, distances[i - 1]);
+ node.ai.mTransformation = mat4ToaiMatrix(rot * aiMatrixToMat4(node.ai.mTransformation));
+
+ /* std::cerr << node.ai.mName.C_Str() << ":\n"; */
+ /* printVec3(extractPos(aiMatrixToMat4(node.ai.mTransformation))); */
+ /* printVec3(newRelPos); */
+ assert(distance(extractPos(aiMatrixToMat4(node.ai.mTransformation)), newRelPos) < 0.0001);
+
+ /* absTrans[3] = vec4(newPositions[i], absTrans[3][3]); // update position in transform */
+
+ /* mat4 relTrans = absoluteToModelSpace(root, *node.parent, absTrans); */
+ /* node.ai.mTransformation = mat4ToaiMatrix(relTrans); */
+ }
+
+ // TODO: Now rotate all the nodes so that they face each other
+
+ /* for (int i = 0; i < 3; i++) { */
+ /* glm::mat4 absTrans(1); */
+ /* findNodeTrans(&sceneModel->getRoot()->ai, aiString(jointNames[i]), */
+ /* &absTrans); */
+ /* glm::mat4 newAbsTrans = absTrans; */
+ /* newAbsTrans[3] = glm::vec4(newPositions[i], newAbsTrans[3][3]); */
+
+ /* auto node = sceneModel->getRoot()->ai.FindNode(jointNames[i].c_str()); */
+
+ /* auto newTrans = worldSpaceToModelSpace(node->mParent, newAbsTrans); */
+
+ /* node->mTransformation = mat4ToaiMatrix(newTrans); */
+ /* } */
+}
+
--- /dev/null
+#include <glm/glm.hpp>
+#include "model.hpp"
+
+void inverseKinematic(Model::Node &end, Model::Node &root, glm::vec3 target);
#include "skybox.hpp"
#include "image.hpp"
#include "util.hpp"
+#include "ik.hpp"
#pragma clang diagnostic ignored "-Wdeprecated-declarations"
glDrawArrays(GL_TRIANGLES, 0, 36);
}
+int findNodeTrans(const struct aiNode *n, const struct aiString name, glm::mat4 *dest) {
+ if (strcmp(n->mName.data, name.data) == 0) {
+ *dest = aiMatrixToMat4(n->mTransformation);
+ return 0;
+ }
+ for (int i = 0; i < n->mNumChildren; i++) {
+ if (findNodeTrans(n->mChildren[i], name, dest) == 0) {
+ glm::mat4 t = aiMatrixToMat4(n->mTransformation);
+ *dest = t * *dest;
+ return 0;
+ }
+ }
+ return 1;
+}
+
+glm::mat4 worldSpaceToModelSpace(aiNode *node, glm::mat4 m) {
+ aiNode *parent = node;
+ glm::mat4 res = m;
+ std::vector<glm::mat4> trans;
+ while (parent != nullptr) {
+ /* res = res * glm::inverse(aiMatrixToMat4(parent->mTransformation)); */
+ trans.push_back(glm::inverse(aiMatrixToMat4(parent->mTransformation)));
+ parent = parent->mParent;
+ }
+ while (!trans.empty()) { res = trans.back() * res; trans.pop_back(); }
+ return res;
+}
void display() {
glClearColor(0.5, 0.5, 0.5, 1);
glUniform3fv(glGetUniformLocation(pbrProg->progId, "lightPositions"), numLights, glm::value_ptr(lightPositions[0]));
glUniform3fv(glGetUniformLocation(pbrProg->progId, "lightColors"), numLights, glm::value_ptr(lightColors[0]));
+ glm::vec3 targetPos(sin(d * 1.f), 0, cos(d * 1.f));
+ inverseKinematic(*sceneModel->find("Bottom Bone"), *sceneModel->find("Toppest Bone"), targetPos);
+
+ /* std::array<glm::vec3, 3> jointPositions; std::array<float, 2> jointDistances; */
+
+ /* std::array<std::string, 3> jointNames = { "Bottom Bone", "Middle Bone", "Top Bone" }; */
+ /* for (int i = 0; i < 3; i++) { */
+ /* glm::mat4 trans; */
+ /* findNodeTrans(&sceneModel->getRoot()->ai, aiString(jointNames[i]), &trans); */
+ /* jointPositions[i] = glm::vec3(trans[3]); */
+
+ /* if (i > 0) */
+ /* jointDistances[i - 1] = glm::distance(jointPositions[i], jointPositions[i - 1]); */
+ /* } */
+
+ /* glm::vec3 targetPos(sin(d * 10.f), cos(d * 10.f), 0); */
+ /* auto newPositions = fabrik(targetPos, jointPositions, jointDistances); */
+
+ /* for (int i = 0; i < 3; i++) { */
+ /* glm::mat4 absTrans(1); */
+ /* findNodeTrans(&sceneModel->getRoot()->ai, aiString(jointNames[i]), */
+ /* &absTrans); */
+ /* glm::mat4 newAbsTrans = absTrans; */
+ /* newAbsTrans[3] = glm::vec4(newPositions[i], newAbsTrans[3][3]); */
+
+ /* auto node = sceneModel->getRoot()->ai.FindNode(jointNames[i].c_str()); */
+
+ /* auto newTrans = worldSpaceToModelSpace(node->mParent, newAbsTrans); */
+
+ /* node->mTransformation = mat4ToaiMatrix(newTrans); */
+ /* } */
/* sceneModel->find("Top Bone")->transform = glm::rotate(glm::mat4(1), d / 5.f, { 1, 0, 0}); */
/* sceneModel->find("Bottom Bone")->transform = glm::rotate(glm::mat4(1), d / 3.f, { 1, 0, 0}); */
+
sceneModel->draw(skyboxes[activeSkybox], d * 1000);
for (Light &light: lights) drawLight(light);
glVertexAttribPointer(posLoc, 3, GL_FLOAT, GL_FALSE, 0, 0);
}
-int findNodeTrans(struct aiNode *n, const struct aiString name, glm::mat4 *dest) {
- if (strcmp(n->mName.data, name.data) == 0) {
- *dest = aiMatrixToMat4(n->mTransformation);
- return 0;
- }
- for (int i = 0; i < n->mNumChildren; i++) {
- if (findNodeTrans(n->mChildren[i], name, dest) == 0) {
- glm::mat4 t = aiMatrixToMat4(n->mTransformation);
- *dest = t * *dest;
- return 0;
- }
- }
- return 1;
-}
void init() {
initUtilProg();
pbrProg = new Program("pbrvert.glsl", "pbrfrag.glsl");
glUseProgram(pbrProg->progId);
- const std::string scenePath = "models/newtonsCradle.glb";
+ const std::string scenePath = "models/ik.glb";
const aiScene *scene = importer.ReadFile(scenePath, aiProcess_Triangulate | aiProcess_CalcTangentSpace | aiProcess_GenNormals | aiProcess_FlipUVs);
if (!scene) {
std::cerr << importer.GetErrorString() << std::endl;
void timer(int _) {
#ifdef ENABLE_MOVEMENT
float xSpeed = 0.f, ySpeed = 0.f, zSpeed = 0.f;
+
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wchar-subscripts"
if (keyStates['w'])
zSpeed = 0.1f;
if (keyStates['s'])
ySpeed = 0.1f;
if (keyStates['e'])
ySpeed = -0.1f;
+#pragma clang diagnostic pop
camPos.x += xSpeed * sin(yaw) + zSpeed * cos(yaw);
camPos.y += ySpeed;
glVertexAttribPointer(boneWeightLoc, 4, GL_FLOAT, GL_FALSE, sizeof(VertBones), (const GLvoid *)sizeof(VertBones::ids));
}
-Model::Node::Node(const aiNode &node, GLuint progId, AnimMap *am): ai(node), progId(progId), animMap(am) {
+Model::Node::Node(aiNode &node, GLuint progId, AnimMap *am, std::set<std::string> allBones, Node *p): ai(node), parent(p), progId(progId), animMap(am), isBone(allBones.count(std::string(node.mName.C_Str())) > 0) {
for (int i = 0; i < node.mNumMeshes; i++) {
meshIndices.push_back(node.mMeshes[i]);
}
for (int i = 0; i < node.mNumChildren; i++) {
- const aiNode *child = node.mChildren[i];
- children.push_back(new Node(*child, progId, am));
+ aiNode *child = node.mChildren[i];
+ children.push_back(new Node(*child, progId, am, allBones, this));
}
}
glm::mat4 m = totalTrans(parentTrans, tick);
#ifdef DEBUG_NODES
+ if (isBone)
+ drawDebugNode(m, {0, 0.5, 1, 1});
+ else
drawDebugNode(m);
#endif
for (Node *child: children) child->draw(meshes, materials, skybox, tick, boneTransforms, m);
}
-void printMatrix4x4(aiMatrix4x4 m) {
- fprintf(stderr, "%f, %f, %f, %f\n", m.a1, m.a2, m.a3, m.a4);
- fprintf(stderr, "%f, %f, %f, %f\n", m.b1, m.b2, m.b3, m.b4);
- fprintf(stderr, "%f, %f, %f, %f\n", m.c1, m.c2, m.c3, m.c4);
- fprintf(stderr, "%f, %f, %f, %f\n", m.d1, m.d2, m.d3, m.d4);
-}
-
void printHierarchy(aiNode *n, int indent = 0) {
for (int i = 0; i < indent; i++)
fprintf(stderr, "\t");
Model::Model(const aiScene *scene, Program p): program(p) {
glUseProgram(p.progId);
+ std::set<std::string> allBones;
for (int i = 0; i < scene->mNumMeshes; i++) {
const aiMesh *mesh = scene->mMeshes[i];
meshes.push_back(Mesh(mesh, p.progId));
+ for (int j = 0; j < mesh->mNumBones; j++)
+ allBones.insert(std::string(mesh->mBones[j]->mName.C_Str()));
}
for (unsigned int i = 0; i < scene->mNumMaterials; i++) {
}
}
- root = new Node(*(scene->mRootNode), p.progId, &animMap);
+ printHierarchy(scene->mRootNode);
+
+ root = new Node(*(scene->mRootNode), p.progId, &animMap, allBones, nullptr);
}
}
return nullptr;
}
+
+bool Model::Node::operator==(const Model::Node &rhs) const {
+ return &ai == &rhs.ai;
+}
+#ifndef MODEL_HPP
+#define MODEL_HPP
+
#include <vector>
#include <map>
#include <set>
return to;
}
+inline aiMatrix4x4 mat4ToaiMatrix(glm::mat4 from) {
+ aiMatrix4x4 to;
+ for (int i = 0; i < 4; i++)
+ for (int j = 0; j < 4; j++)
+ to[i][j] = from[j][i];
+ return to;
+}
+
class Model {
struct Animation {
class Node {
public:
- Node(const aiNode &aiNode, GLuint progId, AnimMap *animMap);
+ Node(aiNode &aiNode, GLuint progId, AnimMap *animMap, std::set<std::string> allBones, Node *parent);
+
void draw(const std::vector<Mesh> &meshes, const std::vector<Material> &materials, const Skybox s, const float tick, const BoneTransforms &boneTransforms, glm::mat4 parentModel) const;
const std::vector<Node*> &getChildren() const { return children; }
Node* findNode(const aiNode &aiNode);
- const aiNode &ai;
+ aiNode &ai;
// an extra transform
glm::mat4 transform = glm::mat4(1);
glm::mat4 totalTrans(const glm::mat4 parentTrans, const float tick) const;
+ const Node *parent;
+
+ bool operator==(const Node &rhs) const;
+
private:
const GLuint progId;
const AnimMap *animMap;
std::vector<Node*> children;
std::vector<unsigned int> meshIndices;
+ const bool isBone;
};
Node* getRoot() { return root; }
BoneTransforms calcBoneTransforms(const Node &n, const float tick, const std::set<std::string> bones, const glm::mat4 parentTrans) const;
void loadModel(const std::string &path);
};
+
+#endif
glUseProgram(prevProg);
}
+
+void printMatrix4x4(aiMatrix4x4 m) {
+ fprintf(stderr, "%f, %f, %f, %f\n", m.a1, m.a2, m.a3, m.a4);
+ fprintf(stderr, "%f, %f, %f, %f\n", m.b1, m.b2, m.b3, m.b4);
+ fprintf(stderr, "%f, %f, %f, %f\n", m.c1, m.c2, m.c3, m.c4);
+ fprintf(stderr, "%f, %f, %f, %f\n", m.d1, m.d2, m.d3, m.d4);
+}
+
+void printVec3(glm::vec3 v) {
+ fprintf(stderr, "{ %f, %f, %f }\n", v[0], v[1], v[2]);
+}
#include "program.hpp"
#include <glm/ext.hpp>
+#include <assimp/scene.h>
void initUtilProg();
Program *getUtilProg();
void drawDebugNode(glm::mat4 transform, glm::vec4 color = {1, 0.5, 1, 1});
+void printMatrix4x4(aiMatrix4x4 m);
+void printVec3(glm::vec3 v);
projects / opengl.git / commitdiff