void Graph::Attach(int from, EdgeRef to) { Collider* col = std::get<2>(to); int toIndex = std::get<0>(to); Plane* face1 = nodes[from].GetPlane(); Plane* face2 = nodes[toIndex].GetPlane(); glm::vec3 face1orig = face1->getPosition(), face2orig = face2->getPosition(), axis = glm::abs(glm::cross(face1orig, face2orig)); auto angle = glm::orientedAngle(face1orig, face2orig,axis); face2->addChild(col); col->ApplySpace(glm::inverse(face2->getWorldMatrix())); col->addChild(face1); face1->ApplySpace(glm::inverse(col->getWorldMatrix())); auto face1pos = face1->getModelPosition(); col->JointRotateBy(angle); }