25 lines
724 B
C++
25 lines
724 B
C++
|
#include <iostream>
|
||
|
|
#include <eigen3/Eigen/Dense>
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
using namespace Eigen;
|
||
|
|
int main() {
|
||
|
|
Vector3d v1(1,1,0), n1(0,0,1);
|
||
|
|
Vector3d p0(120,0,0), p1(2,3,4), p2 = p0;
|
||
|
|
p2.z() += 10;
|
||
|
|
Vector3d v2 = p1 - p0;
|
||
|
|
Vector3d n2 = v2.cross(p2 - p0);
|
||
|
|
//n2.normalize();
|
||
|
|
Matrix3d mt1 = Quaterniond::FromTwoVectors(v1,v2).matrix();
|
||
|
|
n1 = mt1 * n1;
|
||
|
|
Matrix3d mt2 = Quaterniond::FromTwoVectors(n1, n2).matrix();
|
||
|
|
Matrix3d mt = mt2 * mt1;
|
||
|
|
Vector3d scale(v2.x() / v1.x(), v2.y() / v1.y(), v2.z() / v1.z());
|
||
|
|
Vector3d in1(0,1,0), in2(1, 0,0);
|
||
|
|
in1 = mt * in1 * scale;
|
||
|
|
in2 = mt * in2 * scale;
|
||
|
|
char buf[1024];
|
||
|
|
sprintf(buf,"(%f %f %f) (%f %f %f) \n", in1.x(), in1.y(), in1.z()
|
||
|
|
, in2.x(), in2.y(), in2.z());
|
||
|
|
cout << buf;
|
||
|
|
}
|