5axis/rotation.cpp at master · je310/5axis · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
#include "rotation.h"

using namespace irr;


core::vector3df getClosestPointOnLine
( const core::vector3df& axis,const core::vector3df& pivot,const core::vector3df& point){
    core::vector3df c = point - pivot;
    f32 t = axis.dotProduct(c);
    return pivot + axis*t;
}

//both axis and pivot are in world space
void revolveNodeInWorldSpace(scene::ISceneNode* node, f32 degs, const core::vector3df& axis, const core::vector3df& pivot){
    node->updateAbsolutePosition();
    core::vector3df p1 = node->getAbsolutePosition();
    core::vector3df p2 = getClosestPointOnLine(axis, pivot, p1);
    core::vector3df vect = p1 - p2;

    core::quaternion q;
    q.fromAngleAxis(degs*core::DEGTORAD, axis);
    q.getMatrix().rotateVect(vect);

    node->setPosition(p2 + vect);
}

void invrevolveNodeInWorldSpace(scene::ISceneNode* node, f32 degs, const core::vector3df& axis, const core::vector3df& pivot){
    node->updateAbsolutePosition();
    core::vector3df p1 = node->getAbsolutePosition();
    core::vector3df p2 = getClosestPointOnLine(axis, pivot, p1);
    core::vector3df vect = p1 - p2;
	core::CMatrix4<f32> mat ;
    core::quaternion q;
    q.fromAngleAxis(degs*core::DEGTORAD, axis);
	q.getMatrix().getInverse(mat);
	mat.rotateVect(vect);

    node->setPosition(p2 + vect);
}

void rotateNodeInLocalSpace(scene::ISceneNode* node, f32 degs, const core::vector3df& axis)
{
    node->updateAbsolutePosition();
    core::matrix4 m2 = node->getAbsoluteTransformation();
    core::vector3df a = axis;
    m2.rotateVect(a);
    a.normalize();

    core::quaternion q;
    q.fromAngleAxis(degs*core::DEGTORAD, a);
    core::matrix4 m1 = q.getMatrix();

    core::matrix4 m = m1*m2;
    node->setRotation(m.getRotationDegrees());
}

void invrotateNodeInLocalSpace(scene::ISceneNode* node, f32 degs, const core::vector3df& axis)
{
    node->updateAbsolutePosition();
    core::matrix4 m2 = node->getAbsoluteTransformation();
    core::vector3df a = axis;
    m2.rotateVect(a);
    a.normalize();

    core::quaternion q;
    q.fromAngleAxis(degs*core::DEGTORAD, a);
    core::matrix4 m1 = q.getMatrix();
	m1.getInverse(m1);
    core::matrix4 m = m1*m2;
    node->setRotation(m.getRotationDegrees());
}

void moveNodeInLocalSpace(scene::ISceneNode* node, const core::vector3df& distVect)
{
    node->updateAbsolutePosition();
    core::matrix4 m = node->getAbsoluteTransformation();
    core::vector3df d = distVect;
    m.rotateVect(d);

    core::vector3df pos = node->getAbsolutePosition() + d;
    node->setPosition(pos);
}

void revolveNodeInLocalSpace(scene::ISceneNode* node, f32 degs, const core::vector3df& axis, const core::vector3df& pivot)
{
    moveNodeInLocalSpace(node, pivot);
    rotateNodeInLocalSpace(node, degs, axis);
    moveNodeInLocalSpace(node, -pivot);
}