Uni/CGII
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

Merge branch 'master' of ssh://141.30.224.77:23/hodasemi/CGII

Browse source
This commit is contained in:
fruitstaa 2018-06-20 12:12:30 +02:00
parent 823587a538 f931a943ea
commit c2ee2a5909
10 changed files with 897 additions and 599 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

BIN
CG2_SS18_04_IK.pdf Normal file
View file

Binary file not shown.

3
CGII/build/cmake/CMakeLists.txt
View file

@ -33,8 +33,7 @@ cgv_add_module(CGII
../../src/Skeleton.cpp
../../src/SkeletonViewer.cpp
../../src/SkinnedMeshViewer.cpp
../../src/main.cpp
../../src/debughelpers.cpp)
../../src/main.cpp)
# Set include directories
include_directories(

87
CGII/src/AtomicTransform.cpp
View file

@ -92,19 +92,92 @@ T clamp(T &v, T &c1, T &c2)
return v;
}
float angle(const Vec3 &v1, const Vec3 &v2)
{
float d = dot(v1, v2);
float v1_length = length(v1);
float v2_length = length(v2);
return std::acos(d / (v1_length * v2_length));
}
float a(float rcos, float x)
{
return rcos + x * x * (1 - rcos);
}
float b(float rcos, float rsin, float x, float y, float z)
{
return -z * rsin + y * x * (1 - rcos);
}
float c(float rcos, float rsin, float x, float y, float z)
{
return y * rsin + z * x * (1 - rcos);
}
float d(float rcos, float rsin, float x, float y, float z)
{
return z * rsin + x * y * (1 - rcos);
}
float e(float rcos, float y)
{
return rcos + y * y * (1 - rcos);
}
float f(float rcos, float rsin, float x, float y, float z)
{
return -x * rsin + z * y * (1 - rcos);
}
float g(float rcos, float rsin, float x, float y, float z)
{
return -y * rsin + x * z * (1 - rcos);
}
float h(float rcos, float rsin, float x, float y, float z)
{
return x * rsin + y * z * (1 - rcos);
}
float j(float rcos, float z)
{
rcos + z *z *(1 - rcos);
}
float AtomicRotationTransform::least_squares(const Vec3 &local_vector, const Vec3 &target, float r)
{
float angler = r * PI / 180.0f;
float rcos = cos(angler);
float rsin = sin(angler);
float x = axis.x();
float y = axis.y();
float z = axis.z();
float xs = a(rcos, x) * local_vector.x() + b(rcos, rsin, x, y, z) * local_vector.y() + c(rcos, rsin, x, y, z) * local_vector.z();
float ys = d(rcos, rsin, x, y, z) * local_vector.x() + e(rcos, y) * local_vector.y() + f(rcos, rsin, x, y, z) * local_vector.z();
float zs = g(rcos, rsin, x, y, z) * local_vector.x() + h(rcos, rsin, x, y, z) * local_vector.y() + j(rcos, z);
float x_diff = xs - target.x();
float y_diff = ys - target.y();
float z_diff = zs - target.z();
float squares = x_diff * x_diff + y_diff * y_diff + z_diff * z_diff;
}
void AtomicRotationTransform::optimize_value(const Vec3 &local_vector, const Vec3 &target, bool inverse)
{
/*Task: Implement parameter optimization*/
// target into local_target
// get dofs
// project target into plane decided by which dofs exist
// scal mult local_target and local_vector
// optimize this that: target = this->calculate_matrix() * local_vector;
// use only degrees defined by dofs
// if inverse == true use -angle with set_value()
float first_guess = angle(local_vector, target);
double result = 0.0;
double result = least_squares(local_vector, target, first_guess);
if (inverse)
result = -result;

3
CGII/src/AtomicTransform.h
View file

@ -90,6 +90,9 @@ public:
virtual void drawIndicator(float size);
virtual void drawActualIndicator(float size);
private:
virtual float least_squares(const Vec3 &local_vector, const Vec3 &target, float r);
protected:
Vec3 axis;
};

64
CGII/src/IKViewer.cpp
View file

@ -5,7 +5,6 @@
#include "IKViewer.h"
#include "math_helper.h"
#include "debughelpers.h"
#include <unordered_set>
@ -13,6 +12,11 @@
#include <cgv/math/inv.h>
#include <cgv/math/mat.h>
Vec3 into_vec3(const Vec4 &v)
{
return Vec3(v.x(), v.y(), v.z());
}
IKViewer::IKViewer(DataStore *data)
: node("IK Viewer"), data(data), modifying(false), target_position(0, 0, 0, 1), max_iterations(20)
{
@ -97,6 +101,8 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
current_base_matrix = current_base_matrix * transform->calculate_matrix();
}
current_base_matrix = current_base_matrix * base->get_translation_transform_current_joint_to_next();
if (!endeffector)
return;
@ -129,6 +135,9 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
//TODO: check if common ancestor is visited twice
std::shared_ptr<Transform> static_trans = std::shared_ptr<Transform>(new StaticTransform(endeffector->get_translation_transform_current_joint_to_next()));
kinematic_chain.emplace_front(static_trans);
while (1)
{
if (state == 0)
@ -164,16 +173,16 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
Bone *bone = base_tree[base_index];
for (int i = bone->dof_count() - 1; i >= 0; i--)
std::shared_ptr<Transform> inverse_static = std::shared_ptr<Transform>(new StaticTransform(inv(bone->calculate_transform_prev_to_current_without_dofs())));
kinematic_chain.emplace_front(inverse_static);
for (int i = 0; i < bone->dof_count(); i++)
{
Transform *tmp = new InverseTransform(bone->get_dof(i));
std::shared_ptr<Transform> inverse_dof = std::shared_ptr<Transform>(tmp);
kinematic_chain.emplace_front(inverse_dof);
}
std::shared_ptr<Transform> inverse_static = std::shared_ptr<Transform>(new StaticTransform(inv(bone->calculate_transform_prev_to_current_without_dofs())));
kinematic_chain.emplace_front(inverse_static);
base_index--;
}
else
@ -183,17 +192,19 @@ void IKViewer::calculate_kinematic_chain(Bone *base, Bone *endeffector)
}
}
std::shared_ptr<Transform> inverse_static = std::shared_ptr<Transform>(new StaticTransform(inv(base->get_translation_transform_current_joint_to_next())));
kinematic_chain.emplace_front(inverse_static);
current_endeffector_matrix.identity();
kinematic_vector.clear();
for (auto &transform : kinematic_chain)
{
kinematic_vector.emplace_back(transform);
current_endeffector_matrix = current_endeffector_matrix * transform->calculate_matrix();
}
target_position = current_base_matrix * current_endeffector_matrix * endeffector->get_bone_local_tip_position();
print_vec3(target_position);
std::cout << std::endl;
target_position = current_base_matrix * current_endeffector_matrix * Vec4(0.0f, 0.0f, 0.0f, 1.0f);
}
void IKViewer::optimize()
@ -204,15 +215,40 @@ void IKViewer::optimize()
auto skeleton_size = (data->get_skeleton()->getMax() - data->get_skeleton()->getMin());
float distance_threshold = 0.0001f * std::max({skeleton_size.x(), skeleton_size.y(), skeleton_size.z()});
//split the current matrix in:
// before_dof -> dof -> after_dof
/*Task 3.3: Implement CCD */
for (int i = 0; i < max_iterations; i++)
{
for (auto it = kinematic_chain.rbegin(); it != kinematic_chain.rend(); i++)
int kc_size = kinematic_vector.size();
// reverse iterate through kinematic chain
for (int j = kc_size - 1; j >= 0; j--)
{
//it->optimize_value(/*local_vector*/, target_position);
Mat4 before_dof = current_base_matrix;
for (int k = 0; k < j; k++)
{
before_dof = before_dof * kinematic_vector[k]->calculate_matrix();
}
Mat4 after_dof;
after_dof.identity();
int after_dof_index = j + 1;
for (int k = j + 1; k < kc_size; k++)
{
after_dof = after_dof * kinematic_vector[k]->calculate_matrix();
}
// now we got 3 matrices
// (1) before dof: base_matrix * kinematic_vector[0...j-1]
// (2) dof: kinematic_vector[j]
// (3) after_dof: kinematic_vector[j+1...kc_size-1]
auto v_local = after_dof * Vec4(0.0f, 0.0f, 0.0f, 1.0f);
auto v_target = inv(before_dof) * target_position;
kinematic_vector[j]->optimize_value(into_vec3(v_local), into_vec3(v_target));
}
current_endeffector_matrix.identity();

1
CGII/src/IKViewer.h
View file

@ -48,6 +48,7 @@ class IKViewer : public node, public drawable, public provider, public event_han
unsigned int max_iterations;
std::list<std::shared_ptr<Transform>> kinematic_chain;
std::vector<std::shared_ptr<Transform>> kinematic_vector;
void optimize();

1
CGII/src/SkeletonViewer.cpp
View file

@ -12,7 +12,6 @@
#include <cgv/base/find_action.h>
#include "math_helper.h"
#include "debughelpers.h"
using namespace cgv::utils;

23
CGII/src/debughelpers.cpp
View file

@ -1,23 +0,0 @@
#include "debughelpers.h"
#include <iostream>
void print_vec3(const Vec3 &v)
{
std::cout << "(" << v.x() << ", " << v.y() << ", " << v.z() << ")" << std::endl;
}
void print_vec3(const Vec4 &v)
{
print_vec3(into_vec3(v));
}
Vec3 into_vec3(const Vec4 &v)
{
return Vec3(v.x(), v.y(), v.z());
}
Vec4 into_vec4(const Vec3 &v)
{
return Vec4(v.x(), v.y(), v.z(), 1.0f);
}

11
CGII/src/debughelpers.h
View file

@ -1,11 +0,0 @@
#pragma once
#include "math_helper.h"
void print_vec3(const Vec3 &v);
void print_vec3(const Vec4 &v);
Vec3 into_vec3(const Vec4 &v);
Vec4 into_vec4(const Vec3 &v);

523
framework/include/cgv/math/transformations.h
View file

@ -2,18 +2,25 @@
#include "vec.h"
#include "mat.h"
namespace cgv {
namespace math {
namespace cgv
{
namespace math
{
///creates a 3x3 scale matrix
template <typename T>
const mat<T> scale_33(const T &sx, const T &sy, const T &sz)
{
mat<T> m(3, 3);
m(0,0)=sx; m(0,1)= 0; m(0,2)= 0;
m(1,0)=0; m(1,1)=sy; m(1,2)= 0;
m(2,0)=0; m(2,1)= 0; m(2,2)= sz;
m(0, 0) = sx;
m(0, 1) = 0;
m(0, 2) = 0;
m(1, 0) = 0;
m(1, 1) = sy;
m(1, 2) = 0;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = sz;
return m;
}
@ -22,25 +29,33 @@ template <typename T>
const mat<T> scale_33(const T &s)
{
mat<T> m(3, 3);
m(0,0)=s; m(0,1)= 0; m(0,2)= 0;
m(1,0)=0; m(1,1)=s; m(1,2)= 0;
m(2,0)=0; m(2,1)= 0; m(2,2)= s;
m(0, 0) = s;
m(0, 1) = 0;
m(0, 2) = 0;
m(1, 0) = 0;
m(1, 1) = s;
m(1, 2) = 0;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = s;
return m;
}
///creates a 3x3 rotation matrix around the x axis, the angle is in degree
template <typename T>
const mat<T> rotatex_33(const T &angle)
{
T angler = angle * (T)3.14159 / (T)180.0;
mat<T> m(3, 3);
m(0,0)=1; m(0,1)= 0; m(0,2)= 0;
m(1,0)=0; m(1,1)= (T)cos((double)angler);
m(0, 0) = 1;
m(0, 1) = 0;
m(0, 2) = 0;
m(1, 0) = 0;
m(1, 1) = (T)cos((double)angler);
m(1, 2) = -(T)sin((double)angler);
m(2,0)=0; m(2,1)= (T)sin((double)angler);
m(2, 0) = 0;
m(2, 1) = (T)sin((double)angler);
m(2, 2) = (T)cos((double)angler);
return m;
@ -52,13 +67,16 @@ const mat<T> rotatey_33(const T& angle)
{
T angler = angle * (T)3.14159 / (T)180.0;
mat<T> m(3, 3);
m(0,0)=(T)cos((double)angler); m(0,1)= 0;
m(0, 0) = (T)cos((double)angler);
m(0, 1) = 0;
m(0, 2) = (T)sin((double)angler);
m(1,0)=0; m(1,1)=1; m(1,2)= 0;
m(2,0)=-(T)sin((double)angler); m(2,1)= 0;
m(1, 0) = 0;
m(1, 1) = 1;
m(1, 2) = 0;
m(2, 0) = -(T)sin((double)angler);
m(2, 1) = 0;
m(2, 2) = (T)cos((double)angler);
return m;
}
@ -69,10 +87,14 @@ const mat<T> rotatez_33(const T& angle)
T angler = angle * (T)3.14159 / (T)180.0;
mat<T> m(3, 3);
m(0, 0) = (T)cos((double)angler);
m(0,1)= -(T)sin((double)angler); m(0,2)= 0;
m(0, 1) = -(T)sin((double)angler);
m(0, 2) = 0;
m(1, 0) = (T)sin((double)angler);
m(1,1)= (T)cos((double)angler); m(1,2)= 0;
m(2,0)=0; m(2,1)= 0; m(2,2)= 1;
m(1, 1) = (T)cos((double)angler);
m(1, 2) = 0;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = 1;
return m;
}
@ -102,21 +124,17 @@ const mat<T> rotate_33(const T &dirx, const T &diry, const T&dirz,
m(0, 1) = -dirz * rsin + diry * dirx * ((T)1 - rcos);
m(0, 2) = diry * rsin + dirz * dirx * ((T)1 - rcos);
m(1, 0) = dirz * rsin + dirx * diry * ((T)1 - rcos);
m(1, 1) = rcos + diry * diry * ((T)1 - rcos);
m(1, 2) = -dirx * rsin + dirz * diry * ((T)1 - rcos);
m(2, 0) = -diry * rsin + dirx * dirz * ((T)1 - rcos);
m(2, 1) = dirx * rsin + diry * dirz * ((T)1 - rcos);
m(2, 2) = rcos + dirz * dirz * ((T)1 - rcos);
return m;
}
///creates a 3x3 euler rotation matrix from yaw, pitch and roll given in degree
template <typename T>
const mat<T> rotate_euler_33(const T &yaw, const T &pitch, const T &roll)
@ -136,18 +154,14 @@ const mat<T> rotate_euler_33(const T& yaw, const T& pitch,const T& roll)
m(0, 1) = -sinr * cosp;
m(0, 2) = cosr * siny + sinr * sinp * cosy;
m(1, 0) = sinr * cosy + cosr * sinp * siny;
m(1, 1) = cosr * cosp;
m(1, 2) = sinr * siny - cosr * sinp * cosy;
m(2, 0) = -cosp * siny;
m(2, 1) = sinp;
m(2, 2) = cosp * cosy;
return m;
}
@ -160,17 +174,14 @@ const mat<T> star(const vec<T>& v)
m(0, 1) = -v(2);
m(0, 2) = v(1);
m(1, 0) = v(2);
m(1, 1) = 0;
m(1, 2) = -v(0);
m(2, 0) = -v(1);
m(2, 1) = v(0);
m(2, 2) = 0;
return m;
}
@ -200,28 +211,38 @@ const mat<T> rotate_rodrigues_33(const T&rx, const T& ry, const T&rz)
return rotate_rodrigues_33(r);
}
///creates a homogen 3x3 shear matrix with given shears shx in x direction, and shy in y direction
template <typename T>
const mat<T> shearxy_33(const T &shx, const T &shy)
{
mat<T> m(3, 3);
m(0,0)=1; m(0,1)= 0; m(0,2)= shx;
m(1,0)=0; m(1,1)= 1; m(1,2)= shy;
m(2,0)=0; m(2,1)= 0; m(2,2)= 1;
m(0, 0) = 1;
m(0, 1) = 0;
m(0, 2) = shx;
m(1, 0) = 0;
m(1, 1) = 1;
m(1, 2) = shy;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = 1;
return m;
}
///creates a homogen 3x3 shear matrix with given shears shx in x direction, and shy in y direction
template <typename T>
const mat<T> shearxz_33(const T &shx, const T &shz)
{
mat<T> m(3, 3);
m(0,0)=1; m(0,1)= shx; m(0,2)= 0;
m(1,0)=0; m(1,1)= 1; m(1,2)= 0;
m(2,0)=0; m(2,1)= shz; m(2,2)= 1;
m(0, 0) = 1;
m(0, 1) = shx;
m(0, 2) = 0;
m(1, 0) = 0;
m(1, 1) = 1;
m(1, 2) = 0;
m(2, 0) = 0;
m(2, 1) = shz;
m(2, 2) = 1;
return m;
}
@ -230,14 +251,19 @@ template<typename T>
const mat<T> shearyz_33(const T &shy, const T &shz)
{
mat<T> m(3, 3);
m(0,0)=1; m(0,1)= 0; m(0,2)= 0;
m(1,0)=shy; m(1,1)= 1; m(1,2)= 0;
m(2,0)=shz; m(2,1)= 0; m(2,2)= 1;
m(0, 0) = 1;
m(0, 1) = 0;
m(0, 2) = 0;
m(1, 0) = shy;
m(1, 1) = 1;
m(1, 2) = 0;
m(2, 0) = shz;
m(2, 1) = 0;
m(2, 2) = 1;
return m;
}
///creates a homogen 3x3 shear matrix
template <typename T>
const mat<T> shear_33(const T &syx, const T &szx,
@ -245,9 +271,15 @@ const mat<T> shear_33(const T &syx, const T &szx,
const T &sxz, const T &syz)
{
mat<T> m(3, 3);
m(0,0)=1; m(0,1)= syx; m(0,2)= szx;
m(1,0)=sxy; m(1,1)= 1; m(1,2)= szy;
m(2,0)=sxz; m(2,1)= syz; m(2,2)= 1;
m(0, 0) = 1;
m(0, 1) = syx;
m(0, 2) = szx;
m(1, 0) = sxy;
m(1, 1) = 1;
m(1, 2) = szy;
m(2, 0) = sxz;
m(2, 1) = syz;
m(2, 2) = 1;
return m;
}
@ -257,10 +289,22 @@ template <typename T>
const mat<T> translate_44(const T &x, const T &y, const T &z)
{
mat<T> m(4, 4);
m(0,0)=1; m(0,1)= 0; m(0,2)= 0; m(0,3)= x;
m(1,0)=0; m(1,1)= 1; m(1,2)= 0; m(1,3)= y;
m(2,0)=0; m(2,1)= 0; m(2,2)= 1; m(2,3)= z;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = 1;
m(0, 1) = 0;
m(0, 2) = 0;
m(0, 3) = x;
m(1, 0) = 0;
m(1, 1) = 1;
m(1, 2) = 0;
m(1, 3) = y;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = 1;
m(2, 3) = z;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
@ -269,10 +313,22 @@ template <typename T>
const mat<T> translate_44(const vec<T> &v)
{
mat<T> m(4, 4);
m(0,0)=1; m(0,1)= 0; m(0,2)= 0; m(0,3)= v(0);
m(1,0)=0; m(1,1)= 1; m(1,2)= 0; m(1,3)= v(1);
m(2,0)=0; m(2,1)= 0; m(2,2)= 1; m(2,3)= v(2);
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = 1;
m(0, 1) = 0;
m(0, 2) = 0;
m(0, 3) = v(0);
m(1, 0) = 0;
m(1, 1) = 1;
m(1, 2) = 0;
m(1, 3) = v(1);
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = 1;
m(2, 3) = v(2);
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
@ -281,10 +337,22 @@ template <typename T>
const mat<T> scale_44(const T &sx, const T &sy, const T &sz)
{
mat<T> m(4, 4);
m(0,0)=sx; m(0,1)= 0; m(0,2)= 0; m(0,3)= 0;
m(1,0)=0; m(1,1)=sy; m(1,2)= 0; m(1,3)= 0;
m(2,0)=0; m(2,1)= 0; m(2,2)= sz; m(2,3)= 0;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = sx;
m(0, 1) = 0;
m(0, 2) = 0;
m(0, 3) = 0;
m(1, 0) = 0;
m(1, 1) = sy;
m(1, 2) = 0;
m(1, 3) = 0;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = sz;
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
@ -300,27 +368,47 @@ template <typename T>
const mat<T> scale_44(const T &s)
{
mat<T> m(4, 4);
m(0,0)=s; m(0,1)= 0; m(0,2)= 0; m(0,3)= 0;
m(1,0)=0; m(1,1)=s; m(1,2)= 0; m(1,3)= 0;
m(2,0)=0; m(2,1)= 0; m(2,2)= s; m(2,3)= 0;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = s;
m(0, 1) = 0;
m(0, 2) = 0;
m(0, 3) = 0;
m(1, 0) = 0;
m(1, 1) = s;
m(1, 2) = 0;
m(1, 3) = 0;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = s;
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
///creates a 4x4 rotation matrix around the x axis, the angle is in degree
template <typename T>
const mat<T> rotatex_44(const T &angle)
{
T angler = angle * (T)3.14159 / (T)180.0;
mat<T> m(4, 4);
m(0,0)=1; m(0,1)= 0; m(0,2)= 0; m(0,3)= 0;
m(1,0)=0; m(1,1)= (T)cos((double)angler);
m(1,2)= -(T)sin((double)angler); m(1,3)= 0;
m(2,0)=0; m(2,1)= (T)sin((double)angler);
m(2,2)= (T)cos((double)angler); m(2,3)= 0;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = 1;
m(0, 1) = 0;
m(0, 2) = 0;
m(0, 3) = 0;
m(1, 0) = 0;
m(1, 1) = (T)cos((double)angler);
m(1, 2) = -(T)sin((double)angler);
m(1, 3) = 0;
m(2, 0) = 0;
m(2, 1) = (T)sin((double)angler);
m(2, 2) = (T)cos((double)angler);
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
@ -330,12 +418,22 @@ const mat<T> rotatey_44(const T& angle)
{
T angler = angle * (T)3.14159 / (T)180.0;
mat<T> m(4, 4);
m(0,0)=(T)cos((double)angler); m(0,1)= 0;
m(0,2)= (T)sin((double)angler); m(0,3)= 0;
m(1,0)=0; m(1,1)=1; m(1,2)= 0; m(1,3)= 0;
m(2,0)=-(T)sin((double)angler); m(2,1)= 0;
m(2,2)= (T)cos((double)angler); m(2,3)= 0;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = (T)cos((double)angler);
m(0, 1) = 0;
m(0, 2) = (T)sin((double)angler);
m(0, 3) = 0;
m(1, 0) = 0;
m(1, 1) = 1;
m(1, 2) = 0;
m(1, 3) = 0;
m(2, 0) = -(T)sin((double)angler);
m(2, 1) = 0;
m(2, 2) = (T)cos((double)angler);
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
@ -346,15 +444,24 @@ const mat<T> rotatez_44(const T& angle)
T angler = angle * (T)3.14159 / (T)180.0;
mat<T> m(4, 4);
m(0, 0) = (T)cos((double)angler);
m(0,1)= -(T)sin((double)angler); m(0,2)= 0; m(0,3)= 0;
m(0, 1) = -(T)sin((double)angler);
m(0, 2) = 0;
m(0, 3) = 0;
m(1, 0) = (T)sin((double)angler);
m(1,1)= (T)cos((double)angler); m(1,2)= 0; m(1,3)= 0;
m(2,0)=0; m(2,1)= 0; m(2,2)= 1; m(2,3)= 0;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(1, 1) = (T)cos((double)angler);
m(1, 2) = 0;
m(1, 3) = 0;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = 1;
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
template <typename T>
const mat<T> rotate_44(const T &dirx, const T &diry, const T &dirz,
const T &angle)
@ -368,12 +475,10 @@ const mat<T> rotate_44(const T &dirx, const T &diry, const T&dirz,
m(0, 1) = -dirz * rsin + diry * dirx * ((T)1 - rcos);
m(0, 2) = diry * rsin + dirz * dirx * ((T)1 - rcos);
m(1, 0) = dirz * rsin + dirx * diry * ((T)1 - rcos);
m(1, 1) = rcos + diry * diry * ((T)1 - rcos);
m(1, 2) = -dirx * rsin + dirz * diry * ((T)1 - rcos);
m(2, 0) = -diry * rsin + dirx * dirz * ((T)1 - rcos);
m(2, 1) = dirx * rsin + diry * dirz * ((T)1 - rcos);
m(2, 2) = rcos + dirz * dirz * ((T)1 - rcos);
@ -388,7 +493,6 @@ const mat<T> rotate_44(const T &dirx, const T &diry, const T&dirz,
return m;
}
template <typename T>
const mat<T> rotate_33(const cgv::math::vec<T> &dir, const T &angle)
{
@ -409,7 +513,6 @@ const mat<T> rotate_44(const cgv::math::vec<T>& dir, const T& angle)
return rotate_44<T>(vdir(0), vdir(1), vdir(2), angle);
}
///creates a 4x4 euler rotation matrix from yaw, pitch and roll given in degree
template <typename T>
const mat<T> rotate_euler_44(const T &yaw, const T &pitch, const T &roll)
@ -445,29 +548,51 @@ const mat<T> rotate_euler_44(const T& yaw, const T& pitch,const T& roll)
return m;
}
///creates a homogen 4x4 shear matrix with given shears shx in x direction, and shy in y direction
template <typename T>
const mat<T> shearxy_44(const T &shx, const T &shy)
{
mat<T> m(4, 4);
m(0,0)=1; m(0,1)= 0; m(0,2)= shx; m(0,3)= 0;
m(1,0)=0; m(1,1)= 1; m(1,2)= shy; m(1,3)= 0;
m(2,0)=0; m(2,1)= 0; m(2,2)= 1; m(2,3)= 0;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = 1;
m(0, 1) = 0;
m(0, 2) = shx;
m(0, 3) = 0;
m(1, 0) = 0;
m(1, 1) = 1;
m(1, 2) = shy;
m(1, 3) = 0;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = 1;
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
///creates a homogen 4x4 shear matrix with given shears shx in x direction, and shy in y direction
template <typename T>
const mat<T> shearxz_44(const T &shx, const T &shz)
{
mat<T> m(4, 4);
m(0,0)=1; m(0,1)= shx; m(0,2)= 0; m(0,3)= 0;
m(1,0)=0; m(1,1)= 1; m(1,2)= 0; m(1,3)= 0;
m(2,0)=0; m(2,1)= shz; m(2,2)= 1; m(2,3)= 0;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = 1;
m(0, 1) = shx;
m(0, 2) = 0;
m(0, 3) = 0;
m(1, 0) = 0;
m(1, 1) = 1;
m(1, 2) = 0;
m(1, 3) = 0;
m(2, 0) = 0;
m(2, 1) = shz;
m(2, 2) = 1;
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
@ -476,14 +601,25 @@ template<typename T>
const mat<T> shearyz_44(const T &shy, const T &shz)
{
mat<T> m(4, 4);
m(0,0)=1; m(0,1)= 0; m(0,2)= 0; m(0,3)= 0;
m(1,0)=shy; m(1,1)= 1; m(1,2)= 0; m(1,3)= 0;
m(2,0)=shz; m(2,1)= 0; m(2,2)= 1; m(2,3)= 0;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = 1;
m(0, 1) = 0;
m(0, 2) = 0;
m(0, 3) = 0;
m(1, 0) = shy;
m(1, 1) = 1;
m(1, 2) = 0;
m(1, 3) = 0;
m(2, 0) = shz;
m(2, 1) = 0;
m(2, 2) = 1;
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
///creates a homogen 4x4 shear matrix
template <typename T>
const mat<T> shear_44(const T &syx, const T &szx,
@ -491,17 +627,25 @@ const mat<T> shear_44(const T &syx, const T &szx,
const T &sxz, const T &syz)
{
mat<T> m(4, 4);
m(0,0)=1; m(0,1)= syx; m(0,2)= szx; m(0,3)= 0;
m(1,0)=sxy; m(1,1)= 1; m(1,2)= szy; m(1,3)= 0;
m(2,0)=sxz; m(2,1)= syz; m(2,2)= 1; m(2,3)= 0;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = 1;
m(0, 1) = syx;
m(0, 2) = szx;
m(0, 3) = 0;
m(1, 0) = sxy;
m(1, 1) = 1;
m(1, 2) = szy;
m(1, 3) = 0;
m(2, 0) = sxz;
m(2, 1) = syz;
m(2, 2) = 1;
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
///creates a perspective transformation matrix in the same way as gluPerspective does
template <typename T>
const mat<T> perspective_44(const T &fovy, const T &aspect, const T &znear,
@ -510,11 +654,22 @@ const mat<T> perspective_44(const T& fovy, const T&aspect, const T& znear,
T fovyr = (T)(fovy * 3.14159 / 180.0);
T f = (T)(cos(fovyr / 2.0f) / sin(fovyr / 2.0f));
mat<T> m(4, 4);
m(0,0)=f/aspect; m(0,1)= 0; m(0,2)= 0; m(0,3)= 0;
m(1,0)=0; m(1,1)= f; m(1,2)= 0; m(1,3)= 0;
m(2,0)=0; m(2,1)= 0; m(2,2)= (zfar+znear)/(znear-zfar);
m(0, 0) = f / aspect;
m(0, 1) = 0;
m(0, 2) = 0;
m(0, 3) = 0;
m(1, 0) = 0;
m(1, 1) = f;
m(1, 2) = 0;
m(1, 3) = 0;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = (zfar + znear) / (znear - zfar);
m(2, 3) = (2 * zfar * znear) / (znear - zfar);
m(3,0)=0; m(3,1)= 0; m(3,2)= -1; m(3,3)= 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = -1;
m(3, 3) = 0;
return m;
}
@ -526,14 +681,25 @@ mat<T> viewport_44(const T& xoff, const T yoff, const T& width,
mat<T> m(4, 4);
T a = width / (T)2.0;
T b = height / (T)2.0;
m(0,0)=a; m(0,1)= 0; m(0,2)= 0; m(0,3)= xoff+(T)0.5;
m(1,0)=0; m(1,1)= b; m(1,2)= 0; m(1,3)= yoff+(T)0.5;
m(2,0)=0; m(2,1)= 0; m(2,2)= (T)0.5; m(2,3)=(T)0.5 ;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = a;
m(0, 1) = 0;
m(0, 2) = 0;
m(0, 3) = xoff + (T)0.5;
m(1, 0) = 0;
m(1, 1) = b;
m(1, 2) = 0;
m(1, 3) = yoff + (T)0.5;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = (T)0.5;
m(2, 3) = (T)0.5;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
///creates a look at transformation matrix in the same way as gluLookAt does
template <typename T>
const mat<T> look_at_44(const T &eyex, const T &eyey, const T &eyez,
@ -550,10 +716,22 @@ const mat<T> look_at_44(const T &eyex, const T &eyey, const T &eyez,
vec<T> u = normalize(cross(s, f));
mat<T> m(4, 4);
m(0,0)=s(0); m(0,1)=s(1) ; m(0,2)= s(2); m(0,3)= 0;
m(1,0)=u(0); m(1,1)=u(1) ; m(1,2)= u(2); m(1,3)= 0;
m(2,0)=-f(0); m(2,1)= -f(1); m(2,2)= -f(2); m(2,3)= 0;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = s(0);
m(0, 1) = s(1);
m(0, 2) = s(2);
m(0, 3) = 0;
m(1, 0) = u(0);
m(1, 1) = u(1);
m(1, 2) = u(2);
m(1, 3) = 0;
m(2, 0) = -f(0);
m(2, 1) = -f(1);
m(2, 2) = -f(2);
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
m = m * translate_44(-eyex, -eyey, -eyez);
return m;
}
@ -562,17 +740,28 @@ template<typename T>
const mat<T> look_at_44(vec<T> eye, vec<T> center, vec<T> up)
{
vec<T> f = normalize(center - eye);
up = normalize(up);
vec<T> s = normalize(cross(f, up));
vec<T> u = normalize(cross(s, f));
mat<T> m(4, 4);
m(0,0)=s(0); m(0,1)=s(1) ; m(0,2)= s(2); m(0,3)= 0;
m(1,0)=u(0); m(1,1)=u(1) ; m(1,2)= u(2); m(1,3)= 0;
m(2,0)=-f(0); m(2,1)= -f(1); m(2,2)= -f(2); m(2,3)= 0;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = s(0);
m(0, 1) = s(1);
m(0, 2) = s(2);
m(0, 3) = 0;
m(1, 0) = u(0);
m(1, 1) = u(1);
m(1, 2) = u(2);
m(1, 3) = 0;
m(2, 0) = -f(0);
m(2, 1) = -f(1);
m(2, 2) = -f(2);
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
m = m * translate_44(-eye);
return m;
}
@ -590,10 +779,22 @@ const mat<T> frustrum_44(const T& left, const T&right,
T C = -(zfar + znear) / (zfar - znear);
T D = -(2 * zfar * znear) / (zfar - znear);
mat<T> m(4, 4);
m(0,0)=e; m(0,1)= 0; m(0,2)= A; m(0,3)= 0;
m(1,0)=0; m(1,1)= f; m(1,2)= B; m(1,3)= 0;
m(2,0)=0; m(2,1)= 0; m(2,2)= C; m(2,3)= D;
m(3,0)=0; m(3,1)= 0; m(3,2)= -1; m(3,3)= 0;
m(0, 0) = e;
m(0, 1) = 0;
m(0, 2) = A;
m(0, 3) = 0;
m(1, 0) = 0;
m(1, 1) = f;
m(1, 2) = B;
m(1, 3) = 0;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = C;
m(2, 3) = D;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = -1;
m(3, 3) = 0;
return m;
}
@ -610,14 +811,25 @@ const mat<T> ortho_44(const T& left, const T&right,
T ty = (top + bottom) / (top - bottom);
T tz = (zfar + znear) / (zfar - znear);
mat<T> m(4, 4);
m(0,0)=a; m(0,1)= 0; m(0,2)= 0; m(0,3)= -tx;
m(1,0)=0; m(1,1)= b; m(1,2)= 0; m(1,3)= -ty;
m(2,0)=0; m(2,1)= 0; m(2,2)= c; m(2,3)= -tz;
m(3,0)=0; m(3,1)= 0; m(3,2)= 0; m(3,3)= 1;
m(0, 0) = a;
m(0, 1) = 0;
m(0, 2) = 0;
m(0, 3) = -tx;
m(1, 0) = 0;
m(1, 1) = b;
m(1, 2) = 0;
m(1, 3) = -ty;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = c;
m(2, 3) = -tz;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m;
}
///creates an orthographic projection matrix in the same way as glOrtho2d does
template <typename T>
const mat<T> ortho2d_44(const T &left, const T &right,
@ -626,7 +838,6 @@ const mat<T> ortho2d_44(const T& left, const T&right,
return ortho_44<T>(left, right, bottom, top, (T)-1.0, (T)1.0);
}
///creates a picking matrix like gluPickMatrix with pixel (0,0) in the lower left corner if flipy=false
template <typename T>
const mat<T> pick_44(const T &x, const T &y, const T &width, const T &height, int viewport[4], const mat<double> &modelviewproj, bool flipy = true)
@ -642,10 +853,22 @@ const mat<T> pick_44(const T& x,const T& y,const T& width, const T& height,int v
else
ty = (T)(viewport[3] + 2.0 * (viewport[1] - y)) / height;
m(0,0) = sx; m(0,1) = 0; m(0,2) = 0; m(0,3) = tx;
m(1,0) = 0; m(1,1) = sy; m(1,2) = 0; m(1,3) = ty;
m(2,0) = 0; m(2,1) = 0; m(2,2) = 1; m(2,3) = 0;
m(3,0) = 0; m(3,1) = 0; m(3,2) = 0; m(3,3) = 1;
m(0, 0) = sx;
m(0, 1) = 0;
m(0, 2) = 0;
m(0, 3) = tx;
m(1, 0) = 0;
m(1, 1) = sy;
m(1, 2) = 0;
m(1, 3) = ty;
m(2, 0) = 0;
m(2, 1) = 0;
m(2, 2) = 1;
m(2, 3) = 0;
m(3, 0) = 0;
m(3, 1) = 0;
m(3, 2) = 0;
m(3, 3) = 1;
return m * modelviewproj;
}
@ -676,14 +899,15 @@ void decompose_R_2_RxRyRz(const mat<T>& R33,T &angle_x, T& angle_y, T& angle_z)
}
/* return only positive angles in [0,360] */
if (angle_x < 0) angle_x += (T)360;
if (angle_y < 0) angle_y += (T)360;
if (angle_z < 0) angle_z += (T)360;
if (angle_x < 0)
angle_x += (T)360;
if (angle_y < 0)
angle_y += (T)360;
if (angle_z < 0)
angle_z += (T)360;
//std::cout << angle_x << " "<< angle_y << " " <<angle_z <<std::endl;
}
template <typename T>
const mat<T> extract_frustrum_planes(const mat<T> &modelviewprojection)
{
@ -700,10 +924,7 @@ const mat<T> extract_frustrum_planes(const mat<T>& modelviewprojection)
for (unsigned i = 0; i < 6; i++)
frustrum_planes.set_col(i, frustrum_planes.col(i) / length(frustrum_planes.col(i).sub_vec(0, 3)));
return frustrum_planes;
}
}
}
} // namespace math
} // namespace cgv