ikApp.cpp

#include "cinder/app/App.h"
#include "cinder/app/RendererGl.h"
#include "cinder/gl/gl.h"
#include "cinder/CameraUi.h"

using namespace ci;
using namespace ci::app;
using namespace std;

class ikApp : public App {
public:
	void setup() override;
	void mouseMove(MouseEvent event) override;
	void mouseWheel(MouseEvent event) override;
	void mouseDown(MouseEvent event) override;
	void mouseDrag(MouseEvent event) override;
	void keyDown(KeyEvent event) override;
	void update() override;
	void draw() override;
private:
	struct Node
	{
	public:
		Node(int parent, vec3 localTranslation, quat localRotation, vec3 localScale)
			: parent(parent)
			, localTranslation(localTranslation)
			, localRotation(localRotation)
			, localScale(localScale)
		{}

		int parent;
		vec3 localTranslation;
		quat localRotation;
		vec3 localScale;

		mat4 CalcLocalTransform() const
		{
			return glm::translate(localTranslation) * glm::toMat4(localRotation) * glm::scale(localScale);
		}
	};
	void ccdIk(int effectorNodeIdx);
	vector<mat4> calcWorldTransforms(const vector<Node>& nodes);

	vector<Node> nodes_;
	CameraPersp camera_;
	CameraUi cameraUi_;
	vec3 targetPos_;
};

void ikApp::setup()
{
	camera_.lookAt(vec3(0.f, 0.f, 10.f), vec3(0.f));
	cameraUi_.setCamera(&camera_);
	nodes_.emplace_back(-1, vec3(0.f), quat(vec3(0.f)), vec3(1.f));
	nodes_.emplace_back(0, vec3(0.f, 1.f, 0.f), quat(vec3(0.f, 0.f, toRadians(90.f))), vec3(1.f));
	nodes_.emplace_back(1, vec3(0.f, .5f, 0.f), quat(vec3(0.f)), vec3(1.f));
	nodes_.emplace_back(2, vec3(0.f, 0.f, .5f), quat(vec3(0.f)), vec3(1.f));
	nodes_.emplace_back(3, vec3(.5f, 0.f, 0.f), quat(vec3(0.f)), vec3(1.f));
}

void ikApp::mouseMove(MouseEvent event)
{
	cameraUi_.mouseDown(event);
}
void ikApp::mouseWheel(MouseEvent event)
{
	cameraUi_.mouseWheel(event);
}
void ikApp::mouseDrag(MouseEvent event)
{
	cameraUi_.mouseDrag(event);
}
void ikApp::mouseDown( MouseEvent event )
{
	cameraUi_.mouseDown(event);
}
void ikApp::keyDown(KeyEvent event)
{
	switch (event.getCode())
	{
	case KeyEvent::KEY_LEFT:
		targetPos_.x -= 0.1f;
		break;
	case KeyEvent::KEY_RIGHT:
		targetPos_.x += 0.1f;
		break;
	case KeyEvent::KEY_UP:
		if (event.isShiftDown())
		{
			targetPos_.z -= 0.1f;
		}
		else
		{
			targetPos_.y += 0.1f;
		}
		break;
	case KeyEvent::KEY_DOWN:
		if (event.isShiftDown())
		{
			targetPos_.z += 0.1f;
		}
		else
		{
			targetPos_.y -= 0.1f;
		}
		break;
	default:
		break;
	}
}

void ikApp::update()
{
}

void ikApp::draw()
{
	gl::clear( Color( 0.f, 0.f, 0.f ) );
	// turn on z-buffering
	gl::enableDepthRead();
	gl::enableDepthWrite();


	ccdIk(nodes_.size() - 1);
	auto worldTransforms = calcWorldTransforms(nodes_);

	gl::setMatrices(camera_);

	auto def = gl::ShaderDef().color();
	auto lambert = gl::ShaderDef().lambert();
	auto shader = gl::getStockShader(def);
	auto shader2 = gl::getStockShader(lambert);
	for (size_t i = 0; i != nodes_.size(); ++i)
	{
		auto& node = nodes_[i];
		auto& worldTransform = worldTransforms[i];
		auto pos = vec3(worldTransform[3]);
		vec3 xAxisTo(worldTransform * vec4(.2f, 0.f, 0.f, 1.f));
		vec3 yAxisTo(worldTransform * vec4(0.f, .2f, 0.f, 1.f));
		vec3 zAxisTo(worldTransform * vec4(0.f, 0.f, .2f, 1.f));
		shader->bind();
		gl::color(Color(1.f, 0.f, 0.f));
		gl::drawLine(pos, xAxisTo);
		gl::color(Color(0.f, 1.f, 0.f));
		gl::drawLine(pos, yAxisTo);
		gl::color(Color(0.f, 0.f, 1.f));
		gl::drawLine(pos, zAxisTo);
		shader2->bind();
		if (node.parent != -1)
		{
			auto& parentWorldTransform = worldTransforms[node.parent];
			gl::color(Color(1.f, 1.f, 1.f));
			gl::drawVector(vec3(parentWorldTransform[3]), vec3(worldTransform[3]));
		}
	}
	gl::drawSphere(Sphere(targetPos_, 0.1f));
}

void ikApp::ccdIk(int effectorNodeIdx)
{
	//for (size_t i = 0; i != 100; ++i)
	//{
		for (int nodeIdx = nodes_[effectorNodeIdx].parent; nodeIdx != -1; nodeIdx = nodes_[nodeIdx].parent)
		{
			auto worldTransforms = calcWorldTransforms(nodes_);
			auto& node = nodes_[nodeIdx];
			vec3 nodePos(worldTransforms[nodeIdx][3]);
			vec3 effectorPos(worldTransforms[effectorNodeIdx][3]);
			auto toEffectorDir = normalize(effectorPos - nodePos);
			auto toTargerDir = normalize(targetPos_ - nodePos);
			auto rotationDot = dot(toEffectorDir, toTargerDir);
			auto rotationAngle = acos(rotationDot);
			if (rotationAngle > 1.0e-5f)
			{
				auto rotationAxis = normalize(cross(toEffectorDir, toTargerDir));
				auto rotation = angleAxis(rotationAngle, rotationAxis);
				node.localRotation = rotation * node.localRotation;
			}
		}
	//}
}

vector<mat4> ikApp::calcWorldTransforms(const vector<Node>& nodes)
{
	vector<mat4> worldTransforms(nodes_.size());
	for (size_t i = 0; i != nodes_.size(); ++i)
	{
		auto& node = nodes_[i];
		if (node.parent == -1)
		{
			worldTransforms[i] = node.CalcLocalTransform();
		}
		else
		{
			worldTransforms[i] = worldTransforms[node.parent] * node.CalcLocalTransform();
		}
	}
	return worldTransforms;
}

CINDER_APP( ikApp, RendererGl )