void Camera::lookAt(Vec3f eye, Vec3f at, Vec3f up)
{
Vec3f x, y, z;
float glm[16];
/* Make rotation matrix */
/* Z vector */
z = eye - at;
z.normalize();
/* Y vector */
y = up;
/* X vector = Y cross Z */
x = cross_product(y, z);
/* Recompute Y = Z cross X */
y = cross_product(z, x);
/* cross product gives area of parallelogram, which is < 1.0 for
* non-perpendicular unit-length vectors; so normalize x, y here
*/
x.normalize();
y.normalize();
Mat4f m;
memcpy(m[0], x.getPointer(), sizeof(float) * 3);
memcpy(m[1], y.getPointer(), sizeof(float) * 3);
memcpy(m[2], z.getPointer(), sizeof(float) * 3);
m.getGLMatrix(glm);
glMultMatrixf(glm);
/* Translate Eye to Origin */
glTranslated(-eye[0], -eye[1], -eye[2]);
}
t_camera *get_camera(int fd)
{
int r;
t_camera *c;
char *line;
t_vect *diff_btw;
t_vect *look_at;
c = new_camera(NULL, NULL, NULL, NULL);
while ((r = get_next_line(fd, &line)) > 0 && ft_strcmp(line, "----------"))
{
if (!ft_strcmp("pos:", line))
c->campos = get_vector(fd);
if (!ft_strcmp("dir:", line))
look_at = get_vector(fd);
}
if (r == -1)
exit(-1);
diff_btw = new_vector(c->campos->x - look_at->x,
c->campos->y - look_at->y,
c->campos->z - look_at->z);
c->camdir = normalize(negative(diff_btw));
c->camright = normalize(cross_product(new_vector(0, 1, 0), c->camdir));
c->camdown = cross_product(c->camright, c->camdir);
return (c);
}
void lookat_rotate2(double, ex, double ey, double ez, double lx, double ly, double double, ex, double0,double, ex, double1 double, ex, double2,double, ex, double3 double, ex, double4)
{
static Vector N, V, U;
static Vector ep, lp;
ep.x = ex;
ep.y = ey;
ep.z = ez;
lp.x = lx;
lp.y = ly;
lp.z = lz;
points_to_array(&N, &lp, &ep);
normalize_array(&N);
U.x = 0.;
U.y = 1.;
U.z = 0.;
cross_product(&V, &N, &U);
normalize_array(&V);
cross_product(&U, &V, &N);
normalize_array(&U);
M[0] = U.x;
M[1] = V.x;
M[2] = N.x;
M[3] = 0.;
M[4] = U.y;
M[5] = V.y;
M[6] = N.y;
M[7] = 0.;
M[8] = U.z;
M[9] = V.z;
M[10] = N.z;
M[11] = 0.;
#if 0
M[12] = ep.x;
M[13] = ep.y;
M[14] = ep.z;
M[15] = 1.;
#endif
#if 0
M[12] = -U.x * ep.x - U.y * ep.y - U.z * ep.z;
M[13] = -V.x * ep.x - V.y * ep.y - V.z * ep.z;
M[14] = -N.x * ep.x - N.y * ep.y - N.z * ep.z;
#endif
M[12] = ((U.x * ep.x) + (U.y * ep.y) + (U.z * ep.z));
M[13] = ((V.x * ep.x) + (V.y * ep.y) + (V.z * ep.z));
M[14] = ((N.x * ep.x) + (N.y * ep.y) + (N.z * ep.z));
M[15] = 1.;
#ifdef DEBUG
print_array(&lp, "look point");
print_array(&ep, "eye point");
print_array(&N, "normal array");
print_array(&V, "V = N x U");
print_array(&U, "U = V x N");
print_matrix(M, "final matrix");
#endif /* DEBUG */
}
void Mesh::addSlope(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3, float width, unsigned detail,
const Color &color, const Vector3 &up)
{
Vector3 unit_up = normalize(up);
Vector3 unit_forward = normalize(p2 - p1);
Vector3 unit_right = normalize(cross_product(unit_forward, unit_up));
float len = length(p3 - p2);
Vector3 unit_slope = normalize(p3 - p2);
Vector3 unit_slope_right = normalize(cross_product(unit_slope, unit_up));
unsigned index_base = static_cast<unsigned>(m_vertices.getSize());
this->addVertex(p1 - unit_right * width, color);
this->addVertex(p1 + unit_right * width, color);
this->addFace(index_base + 0, index_base + 1, index_base + 3, index_base + 2);
for(unsigned ii = 0; (ii <= detail); ++ii)
{
float fraction = static_cast<float>(ii) / static_cast<float>(detail);
Vector3 forward_component = mix(unit_forward * len * fraction, unit_slope * len * fraction, fraction);
Vector3 right_component = mix(unit_right * width, unit_slope_right * width, fraction);
this->addVertex(p2 + forward_component - right_component, color);
this->addVertex(p2 + forward_component + right_component, color);
unsigned base = index_base + ii * 2;
this->addFace(base + 0, base + 1, base + 3, base + 2);
}
}
bool LabelPosition::isBorderCrossingLine( PointSet* feat )
{
double ca, cb;
for ( int i = 0; i < 4; i++ )
{
for ( int j = 0; j < feat->getNumPoints() - 1; j++ )
{
ca = cross_product( x[i], y[i], x[( i+1 ) %4], y[( i+1 ) %4],
feat->x[j], feat->y[j] );
cb = cross_product( x[i], y[i], x[( i+1 ) %4], y[( i+1 ) %4],
feat->x[j+1], feat->y[j+1] );
if (( ca < 0 && cb > 0 ) || ( ca > 0 && cb < 0 ) )
{
ca = cross_product( feat->x[j], feat->y[j], feat->x[j+1], feat->y[j+1],
x[i], y[i] );
cb = cross_product( feat->x[j], feat->y[j], feat->x[j+1], feat->y[j+1],
x[( i+1 ) %4], y[( i+1 ) %4] );
if (( ca < 0 && cb > 0 ) || ( ca > 0 && cb < 0 ) )
return true;
}
}
}
if ( nextPart )
return nextPart->isBorderCrossingLine( feat );
return false;
}
/* get_align_matrix:
* Aligns a matrix along an arbitrary coordinate system.
*/
void get_align_matrix(MATRIX *m, fixed xfront, fixed yfront, fixed zfront, fixed xup, fixed yup, fixed zup)
{
fixed xright, yright, zright;
ASSERT(m);
xfront = -xfront;
yfront = -yfront;
zfront = -zfront;
normalize_vector(&xfront, &yfront, &zfront);
cross_product(xup, yup, zup, xfront, yfront, zfront, &xright, &yright, &zright);
normalize_vector(&xright, &yright, &zright);
cross_product(xfront, yfront, zfront, xright, yright, zright, &xup, &yup, &zup);
/* No need to normalize up here, since right and front are perpendicular and normalized. */
m->v[0][0] = xright;
m->v[0][1] = xup;
m->v[0][2] = xfront;
m->v[1][0] = yright;
m->v[1][1] = yup;
m->v[1][2] = yfront;
m->v[2][0] = zright;
m->v[2][1] = zup;
m->v[2][2] = zfront;
m->t[0] = m->t[1] = m->t[2] = 0;
}
//returns true, if a point is outside of the closed polygon
bool outside_test(int a){
for (int i = 0; i < n-1; i++){
if (cross_product(test_points[a], vert[i], vert[i+1]) > 0)
angle = angle + dot_product_angle(test_points[a], vert[i], vert[i+1]);
else
angle = angle - dot_product_angle(test_points[a], vert[i], vert[i+1]);
}
if (cross_product(test_points[a], vert[n-1], vert[0]) > 0)
angle = angle + dot_product_angle(test_points[a], vert[n-1], vert[0]);
else
angle = angle - dot_product_angle(test_points[a], vert[n-1], vert[0]);
if (abs(angle) <= .01){
//debugging comment
//std:: cout<<"total angle is "<<angle<<". Color should be red"<<std:: endl;
angle = 0;
return true;
}
else{
//debugging comment
//std:: cout<<"total angle is "<<angle<<". Color should be green"<<std:: endl;
angle = 0;
return false;
}
}
inline bool point_in_triangle( const Point1& p, const Point2& A, const Point3& B, const Point4& C, const NumberComparisonPolicy& cmp, dimension<3> )
{
typedef typename select_arithmetic_type_from_sequences<Point1, Point2>::type arithmetic_type;
// Translate point and triangle so that point lies at origin
vector<arithmetic_type, 3> a = A - p;
vector<arithmetic_type, 3> b = B - p;
vector<arithmetic_type, 3> c = C - p;
// Compute normal vectors for triangles pab and pbc
auto u = cross_product( b, c );
auto v = cross_product( c, a );
// Make sure they are both pointing in the same direction
if( cmp.less_than(dot_product(u, v), constants::zero<decltype(dot_product(u,v))>() ) )
return false;
// Compute normal vector for triangle pca
auto w = cross_product(a, b);
// Make sure it points in the same direction as the first two
if( cmp.less_than(dot_product(u, w), constants::zero<decltype(dot_product(u,w))>() ) )
return false;
// Otherwise P must be in (or on) the triangle
return true;
}
bool lines_intersect(int line1[2][2], int line2[2][2]) {
int line1Vector_x = line1[1][0] - line1[0][0];
int line1Vector_y = line1[1][1] - line1[0][1];
int line2Vector_x = line2[1][0] - line2[0][0];
int line2Vector_y = line2[1][1] - line2[0][1];
int diff_x = line2[0][0] - line1[0][0];
int diff_y = line2[0][1] - line1[0][1];
long cross_product1 = cross_product(line1Vector_x, line1Vector_y, line2Vector_x, line2Vector_y);
long cross_product2 = cross_product(diff_x, diff_y, line1Vector_x, line1Vector_y);
double frac, lambda1, lambda2;
if (cross_product1 == 0) {
/* lines are parallel check whether they coincide */
if (cross_product2 == 0) {
/* parallel and co-linear */
frac = dot_product(line1Vector_x, line1Vector_y, line1Vector_x, line1Vector_y);
lambda1 = dot_product(diff_x, diff_y, line1Vector_x, line1Vector_y) / frac;
lambda2 = lambda1 + dot_product(line1Vector_x, line1Vector_y, line2Vector_x, line2Vector_y) / frac;
if ((0 <= lambda1 && lambda1 <= 1) ||
(0 <= lambda2 && lambda2 <= 1) ||
(lambda1 < 0 && 1 < lambda2) ||
(lambda2 < 0 && 1 < lambda1)) {
return true;
}
return false;
} else {
/* parallel and non intersecting */
return false;
}
} else {
frac = 1. / cross_product1;
}
lambda1 = cross_product(diff_x, diff_y, line2Vector_x, line2Vector_y) * frac;
lambda2 = cross_product(diff_x, diff_y, line1Vector_x, line1Vector_y) * frac;
return ((0 <= lambda1 && lambda1 < 1) && (0 <= lambda2 && lambda2 < 1));
}
float calcDihed(vector<float> &a, vector<float> &b, vector<float> &c, vector<float> &d) {
//cout << "a " << a[0] <<" "<< a[1] <<" "<< a[2] << endl;
//cout << "b " << b[0] <<" "<< b[1] <<" "<< b[2] << endl;
//cout << "c " << c[0] <<" "<< c[1] <<" "<< c[2] << endl;
//cout << "d " << d[0] <<" "<< d[1] <<" "<< d[2] << endl;
vector<float> ba(3), bc(3), cb(3), cd(3);
point_sub(ba, a, b); point_sub(bc, c, b);
point_sub(cb, b, c); point_sub(cd, d, c);
//cout << "ba " << ba[0] <<" "<< ba[1] <<" "<< ba[2] << endl;
//cout << "bc " << bc[0] <<" "<< bc[1] <<" "<< bc[2] << endl;
//cout << "cb " << cb[0] <<" "<< cb[1] <<" "<< cb[2] << endl;
//cout << "cd " << cd[0] <<" "<< cd[1] <<" "<< cd[2] << endl;
vector<float> ba_bc(3), cb_cd(3);
cross_product(cb_cd, cb, cd); normalize_point(cb_cd, cb_cd);
cross_product(ba_bc, ba, bc); normalize_point(ba_bc, ba_bc);
//cout << "cb_cd " << cb_cd[0] <<" "<< cb_cd[1] <<" "<< cb_cd[2] << endl;
//cout << "ba_bc " << ba_bc[0] <<" "<< ba_bc[1] <<" "<< ba_bc[2] << endl;
float dp = dot_product(cb_cd, ba_bc);
if(dp > 1) dp = 1;
if(dp < -1) dp = -1;
float angle = RADIANS_TO_DEGREES ( acos(dp) );
//cout << angle <<" "<< dp << endl;
vector<float> cp(3); cross_product(cp, ba_bc,cb_cd);
if ( dot_product(cp,bc) < 0 ) angle = -1*angle;
return angle;
}
int determinant (MATRIX const *m, float *result)
/*****************************************************************************
******************************************************************************/
{
if (!M_SQUARE (*m))
{
printf ("ERROR (determinant): MATRIX must be square!\n");
print_matrix ("MATRIX:", m);
return -1;
}
else
{
if (MROWS (*m) == 1)
*result = MDATA (*m, 0, 0);
else if (MROWS (*m) == 2)
*result = cross_product (m, 0, 0, 1, 1);
else
*result = MDATA (*m, 0, 0) * cross_product (m, 1, 1, 2, 2)
- MDATA (*m, 0, 1) * cross_product (m, 1, 0, 2, 2)
+ MDATA (*m, 0, 2) * cross_product (m, 1, 0, 2, 1);
return 1;
}
}
void lookat_rotate(Matrix T, double x, double y, double z, Matrix Matrix0)
{
static Vector N, V, U;
static Vector ep, lp;
lp.x = x;
lp.y = y;
lp.z = z;
ep.x = T[12];
ep.y = T[13];
ep.z = T[14];
points_to_array(&N, &lp, &ep);
normalize_array(&N);
U.x = T[0];
U.y = T[4];
U.z = T[8];
cross_product(&V, &N, &U);
normalize_array(&V);
cross_product(&U, &V, &N);
normalize_array(&U);
M[0] = U.x;
M[1] = V.x;
M[2] = N.x;
M[3] = 0.;
M[4] = U.y;
M[5] = V.y;
M[6] = N.y;
M[7] = 0.;
M[8] = U.z;
M[9] = V.z;
M[10] = N.z;
M[11] = 0.;
#if 0
M[12] = ep.x;
M[13] = ep.y;
M[14] = ep.z;
M[15] = 1.;
#endif
#if 0
M[12] = -U.x * ep.x - U.y * ep.y - U.z * ep.z;
M[13] = -V.x * ep.x - V.y * ep.y - V.z * ep.z;
M[14] = -N.x * ep.x - N.y * ep.y - N.z * ep.z;
#endif
M[12] = ((U.x * ep.x) + (U.y * ep.y) + (U.z * ep.z));
M[13] = ((V.x * ep.x) + (V.y * ep.y) + (V.z * ep.z));
M[14] = ((N.x * ep.x) + (N.y * ep.y) + (N.z * ep.z));
M[15] = 1.;
#ifdef DEBUG
print_array(&lp, "look point");
print_array(&ep, "eye point");
print_array(&N, "normal array");
print_array(&V, "V = N x U");
print_array(&U, "U = V x N");
print_matrix(M, "final matrix");
#endif /* DEBUG */
}
bool tri::intersect(ray & _r, float & t) {
//calculate t first
t = inner_product(p[0] - _r.origin, nrml) / inner_product(_r.dir, nrml);
if (t < 0)
return false;
//check inside the tri
point x = _r.get_t(t);
vect v2_1 = p[1] - p[0];
vect v3_2 = p[2] - p[1];
vect v1_3 = p[0] - p[2];
vect vx_1 = x - p[0];
vect vx_2 = x - p[1];
vect vx_3 = x - p[2];
/*special judge for debug
if(abs(x.x) > 500 || abs(x.z) > 500)
return false;
return true;
*/
if (inner_product(cross_product(v2_1, vx_1), nrml) > 0
&& inner_product(cross_product(v3_2, vx_2), nrml) > 0
&& inner_product(cross_product(v1_3, vx_3), nrml) > 0) {
return true;
}
return false;
}
void setup_view_matrix( player_data_t *plyr )
{
vector_t view_x, view_y, view_z;
matrixgl_t view_mat;
point_t viewpt_in_view_frame;
view_z = scale_vector( -1, plyr->view.dir );
view_x = cross_product( plyr->view.up, view_z );
view_y = cross_product( view_z, view_x );
normalize_vector( &view_z );
normalize_vector( &view_x );
normalize_vector( &view_y );
make_identity_matrix( plyr->view.inv_view_mat );
plyr->view.inv_view_mat[0][0] = view_x.x;
plyr->view.inv_view_mat[0][1] = view_x.y;
plyr->view.inv_view_mat[0][2] = view_x.z;
plyr->view.inv_view_mat[1][0] = view_y.x;
plyr->view.inv_view_mat[1][1] = view_y.y;
plyr->view.inv_view_mat[1][2] = view_y.z;
plyr->view.inv_view_mat[2][0] = view_z.x;
plyr->view.inv_view_mat[2][1] = view_z.y;
plyr->view.inv_view_mat[2][2] = view_z.z;
plyr->view.inv_view_mat[3][0] = plyr->view.pos.x;
plyr->view.inv_view_mat[3][1] = plyr->view.pos.y;
plyr->view.inv_view_mat[3][2] = plyr->view.pos.z;
plyr->view.inv_view_mat[3][3] = 1;
transpose_matrix( plyr->view.inv_view_mat, view_mat );
view_mat[0][3] = 0;
view_mat[1][3] = 0;
view_mat[2][3] = 0;
viewpt_in_view_frame = transform_point( view_mat, plyr->view.pos );
view_mat[3][0] = -viewpt_in_view_frame.x;
view_mat[3][1] = -viewpt_in_view_frame.y;
view_mat[3][2] = -viewpt_in_view_frame.z;
glLoadIdentity();
#ifdef __APPLE__DISABLED__
GLfloat matrix[3][3];
int i,j;
for( i = 0; i < 3; i++ )
{
for( j = 0; j < 3; j++ )
matrix[i][j] = view_mat[i][j];
}
glMultMatrixf( (GLfloat *) matrix );
#else
glMultMatrixd( (scalar_t *) view_mat );
#endif
}
double area() const
{
double uv = cross_product(u, v).length();
double uw = cross_product(u, w).length();
double vw = cross_product(v, w).length();
return 2*(uv + uw + vw);
}
void RefMap::calc_face_normal(int iface, const int np, const QuadPt3D *pt, double *&nx, double *&ny, double *&nz) {
_F_
assert(mesh != NULL);
double3x3 *m = get_ref_map(np, pt);
nx = new double[np]; MEM_CHECK(nx);
ny = new double[np]; MEM_CHECK(ny);
nz = new double[np]; MEM_CHECK(nz);
// FIXME: refactor this!
// - it would be nice if calculation of normals would be based on the same algorithm for all elements
// e.g. to take a normal from ref. domain and transform it to the physical one
int t_dir_1, t_dir_2; //directions of tangents ot the reference face such that t_dir_1 x t_dir_2 = outer normal
switch (element->get_mode()) {
case MODE_TETRAHEDRON: {
const int *face_vtx = element->get_face_vertices(iface);
Vertex vtx[Tri::NUM_VERTICES];
for (int i = 0; i < element->get_num_vertices(); i++)
vtx[i] = vertex[face_vtx[i]];
Point3D v1 = { vtx[1].x - vtx[0].x, vtx[1].y - vtx[0].y, vtx[1].z - vtx[0].z };
Point3D v2 = { vtx[2].x - vtx[0].x, vtx[2].y - vtx[2].y, vtx[2].z - vtx[0].z };
Point3D n = normalize(cross_product(v1, v2));
for (int i = 0; i < np; i++) {
nx[i] = n.x;
ny[i] = n.y;
nz[i] = n.z;
}
} break;
case MODE_HEXAHEDRON:
switch (iface) {
case 0: t_dir_1 = 2; t_dir_2 = 1; break;
case 1: t_dir_1 = 1; t_dir_2 = 2; break;
case 2: t_dir_1 = 0; t_dir_2 = 2; break;
case 3: t_dir_1 = 2; t_dir_2 = 0; break;
case 4: t_dir_1 = 1; t_dir_2 = 0; break;
case 5: t_dir_1 = 0; t_dir_2 = 1; break;
}
for (int i = 0; i < np; i++) {
Point3D tangent1 = { m[i][0][t_dir_1], m[i][1][t_dir_1], m[i][2][t_dir_1] };
Point3D tangent2 = { m[i][0][t_dir_2], m[i][1][t_dir_2], m[i][2][t_dir_2] };
Point3D normal = normalize(cross_product(tangent1, tangent2));
nx[i] = normal.x;
ny[i] = normal.y;
nz[i] = normal.z;
}
break;
case MODE_PRISM:
EXIT(HERMES_ERR_NOT_IMPLEMENTED);
}
delete [] m;
}
Point intersection_point(Line l1,Line l2)
{
double s1 = cross_product(l1.a, l1.b, l2.a);
double s2 = cross_product(l1.a, l1.b, l2.b);
Point ret;
ret.x = (s1 * l2.b.x - s2 * l2.a.x) / (s1 - s2);
ret.y = (s1 * l2.b.y - s2 * l2.a.y) / (s1 - s2);
return ret;
}
/*
* Constructor.
*/
Camera::Camera(const Vector3d& _origin, const Vector3d& direction, const Vector3d& up,
double _focal_distance)
: origin(_origin), focal_distance(_focal_distance)
{
// Construct a right handed orthonormal basis.
w = (-direction).normalize();
u = cross_product(up, w).normalize();
v = cross_product(w, u);
}
void setup_view_matrix( player_data_t *plyr )
{
GLfloat matrix[4][4];
int i,j;
vector_t view_x, view_y, view_z;
matrixgl_t view_mat;
point_t viewpt_in_view_frame;
view_z = scale_vector( -1, plyr->view.dir );
view_x = cross_product( plyr->view.up, view_z );
view_y = cross_product( view_z, view_x );
normalize_vector( &view_z );
normalize_vector( &view_x );
normalize_vector( &view_y );
make_identity_matrix( plyr->view.inv_view_mat );
plyr->view.inv_view_mat[0][0] = view_x.x;
plyr->view.inv_view_mat[0][1] = view_x.y;
plyr->view.inv_view_mat[0][2] = view_x.z;
plyr->view.inv_view_mat[1][0] = view_y.x;
plyr->view.inv_view_mat[1][1] = view_y.y;
plyr->view.inv_view_mat[1][2] = view_y.z;
plyr->view.inv_view_mat[2][0] = view_z.x;
plyr->view.inv_view_mat[2][1] = view_z.y;
plyr->view.inv_view_mat[2][2] = view_z.z;
plyr->view.inv_view_mat[3][0] = plyr->view.pos.x;
plyr->view.inv_view_mat[3][1] = plyr->view.pos.y;
plyr->view.inv_view_mat[3][2] = plyr->view.pos.z;
plyr->view.inv_view_mat[3][3] = 1;
transpose_matrix( plyr->view.inv_view_mat, view_mat );
view_mat[0][3] = 0;
view_mat[1][3] = 0;
view_mat[2][3] = 0;
viewpt_in_view_frame = transform_point( view_mat, plyr->view.pos );
view_mat[3][0] = -viewpt_in_view_frame.x;
view_mat[3][1] = -viewpt_in_view_frame.y;
view_mat[3][2] = -viewpt_in_view_frame.z;
//glLoadIdentity();
for( i = 0; i < 4; i++ )
{
for( j = 0; j < 4; j++ )
matrix[i][j] = view_mat[i][j];
}
util_set_view(matrix);
}
/*!
Interpolates between camera orientations
\pre p_up2, p_dir2 != NULL; *p_up2 is the target up vector;
*p_dir2 is the target view direction
\arg \c up1 original up vector
\arg \c dir1 original view direction
\arg \c p_up2 pointer to target up vector; is overwritten with
interpolated up vector
\arg \c p_dir2 pointer to target view direction; is overwritten with
interpolated view direction
\arg \c dt time step
\arg \c time_constant interpolation time constant
\return none
\author jfpatry
\date Created: 2000-08-26
\date Modified: 2000-08-26
*/
void interpolate_view_frame( vector_t up1, vector_t dir1,
vector_t *p_up2, vector_t *p_dir2,
scalar_t dt, scalar_t time_constant )
{
quaternion_t q1, q2;
scalar_t alpha;
vector_t x1, y1, z1;
vector_t x2, y2, z2;
matrixgl_t cob_mat1, inv_cob_mat1;
matrixgl_t cob_mat2, inv_cob_mat2;
/* Now interpolate between camera orientations */
z1 = scale_vector( -1.0, dir1 );
normalize_vector( &z1 );
y1 = project_into_plane( z1, up1 );
normalize_vector( &y1 );
x1 = cross_product( y1, z1 );
make_change_of_basis_matrix( cob_mat1, inv_cob_mat1,
x1, y1, z1 );
q1 = make_quaternion_from_matrix( cob_mat1 );
z2 = scale_vector( -1.0, *p_dir2 );
normalize_vector( &z2 );
y2 = project_into_plane( z2, *p_up2 );
normalize_vector( &y2 );
x2 = cross_product( y2, z2 );
make_change_of_basis_matrix( cob_mat2, inv_cob_mat2,
x2, y2, z2 );
q2 = make_quaternion_from_matrix( cob_mat2 );
alpha = min( MAX_INTERPOLATION_VALUE,
1.0 - exp( -dt / time_constant ) );
q2 = interpolate_quaternions( q1, q2, alpha );
make_matrix_from_quaternion( cob_mat2, q2 );
p_up2->x = cob_mat2[1][0];
p_up2->y = cob_mat2[1][1];
p_up2->z = cob_mat2[1][2];
p_dir2->x = -cob_mat2[2][0];
p_dir2->y = -cob_mat2[2][1];
p_dir2->z = -cob_mat2[2][2];
}
int inverse_matrix (MATRIX const *m, MATRIX *n)
/*****************************************************************************
******************************************************************************/
{
if (!M_SQUARE (*m))
{
printf ("ERROR (inverse_matrix): MATRIX must be square!\n");
print_matrix ("MATRIX:", m);
n->cols=0; n->rows=0;
return -1;
}
else
{
float det;
int res;
res = determinant (m,&det);
if (res == -1)
{
printf ("ERROR (inverse_matrix): singular MATRIX!\n");
print_matrix ("MATRIX:", m);
return -1;
}
else
{
initialize_matrix (n, MROWS (*m), MCOLS (*m));
if (MROWS (*m) == 1)
{
MDATA (*n, 0, 0) = 1 / det ;
}
else if (MROWS (*m) == 2)
{
MDATA (*n, 0, 0) = MDATA (*m, 1, 1) / det ;
MDATA (*n, 0, 1) = -MDATA (*m, 0, 1) / det ;
MDATA (*n, 1, 0) = -MDATA (*m, 1, 0) / det ;
MDATA (*n, 1, 1) = MDATA (*m, 0, 0) / det ;
}
else
{
MDATA (*n, 0, 0) = cross_product (m, 1, 1, 2, 2) / det ;
MDATA (*n, 0, 1) = -cross_product (m, 0, 1, 2, 2) / det ;
MDATA (*n, 0, 2) = cross_product (m, 0, 1, 1, 2) / det ;
MDATA (*n, 1, 0) = -cross_product (m, 1, 0, 2, 2) / det ;
MDATA (*n, 1, 1) = cross_product (m, 0, 0, 2, 2) / det ;
MDATA (*n, 1, 2) = -cross_product (m, 0, 0, 1, 2) / det ;
MDATA (*n, 2, 0) = cross_product (m, 1, 0, 2, 1) / det ;
MDATA (*n, 2, 1) = -cross_product (m, 0, 0, 2, 1) / det ;
MDATA (*n, 2, 2) = cross_product (m, 0, 0, 1, 1) / det ;
}
}
}
return 1;
}
void FindConvex() {
anslen = 0;
for(int i=0;i<len;i++) {
while(anslen >= 2 && cross_product(ans[anslen-2],ans[anslen-1],i) < 0) anslen--;
ans[anslen++] = i;
}
int t = anslen+1;
for(int i=len-2;i>=0;i--) {
while(anslen >= t && cross_product(ans[anslen-2],ans[anslen-1],i) < 0) anslen--;
ans[anslen++] = i;
}
}
bool isTriangleHit( vect3d vertices[3], Ray &ray,
float *pt, float *pu, float *pv)
{
//
// Real-Time Rendering 2nd, 13.7.2
//
vect3d e1; points2vec(vertices[0], vertices[1], e1);
vect3d e2; points2vec(vertices[0], vertices[2], e2);
vect3d p; cross_product(ray.direction_vec, e2, p);
float a = dot_product(e1, p);
if(a > -epsi && a < epsi)
{
return false;
}
float f = 1.f / a;
vect3d s; points2vec(vertices[0], ray.start_point, s);
float u = f * dot_product(s, p);
if(u < 0.f || u > 1.f)
{
return false;
}
vect3d q; cross_product(s, e1, q);
float v = f * dot_product(ray.direction_vec, q);
if(v < 0.f || (u + v) > 1.f)
{
return false;
}
float t = f * dot_product(e2, q);
if(t <= epsi)
{
return false;
}
*pt = t;
if(pu)
{
*pu = u;
}
if(pv)
{
*pv = v;
}
return true;
}
void calibration(PointKAIST *pt_left, PointKAIST *pt_right, int WIDTH, int HEIGHT)
{
/////// point 저장 변수
PointKAIST temp_pt_left[3] = {0,}, temp_pt_right[3] = {0,};
PointKAIST temp_left, temp_right;
int points_num = 0;
int x_left=0, x_right=0;
/////// line 파라미터 저장 변수
int left[3], left_s[3], left_line[3];
int right[3], right_s[3], right_line[3];
int left_right[3];
int r_length = 0, l_length = 0;
left_s[0] = pt_left[0].x; left_s[1] = pt_left[0].y; left_s[2] = 1;
left[0] = pt_left[2].x; left[1] = pt_left[2].y; left[2] = 1;
right_s[0] = pt_right[0].x; right_s[1] = pt_right[0].y; right_s[2] = 1;
right[0] = pt_right[2].x; right[1] = pt_right[2].y; right[2] = 1;
cross_product(left, left_s, left_line);
cross_product(right, right_s, right_line);
cross_product(left, left_s, left_line);
cross_product(right, right_s, right_line);
cross_product(left_line, right_line, left_right);
int cal_center_x = 0, cal_center_y = 0;
if(left_right[2] != 0)
{
cal_center_x = left_right[0] / left_right[2];
cal_center_y = left_right[1] / left_right[2];
}
if(cal_center_x > 0 && cal_center_y > 0 && center_bin[cal_center_x / 10][cal_center_y / 10] < 10000)
{
center_bin[cal_center_x / 10][cal_center_y / 10]++;
}
int pos_x, pos_y;
cal_max_position1(&pos_x, &pos_y);
horizontal_center = pos_y*10+5;
vertical_center = pos_x*10+5;
center = vertical_center;
}
mat_4D look_at(vec_3D p, vec_3D l, vec_3D v)
{
vec_3D n, u;
n = normalise(vector_sub(p,l));
u = normalise(cross_product(v,n));
v = cross_product(n,u);
mat_4D rot = {{ u.x, u.y, u.z, 0,
v.x, v.y, v.z, 0,
n.x, n.y, n.z, 0,
0, 0, 0, 1 }};
mat_4D trans = translation_comp(-p.x, -p.y, -p.z);
return mult(rot, trans);
}
pcl::PointXYZ crossProduct(const pcl::PointXYZ &a, const pcl::PointXYZ &b)
{
pcl::PointXYZ cross_product(a.y * b.z - a.z * b.y,
a.z * b.x - a.x * b.z,
a.x * b.y - a.y * b.x);
return cross_product;
}
plane( const point_type& a, const point_type& b, const point_type& c )
: m_a( a )
, m_u( b-a )
, m_v( c-a )
, m_n( normalize( cross_product( m_u, m_v ) ) )
, m_d( dot_product( m_n, as_vector( m_a ) ) )
{}
plane( const point_type& a, const vector_type& u, const vector_type& v )
: m_a( a )
, m_u( u )
, m_v( v )
, m_n( normalize( cross_product( m_u, m_v ) ) )
, m_d( dot_product(m_n, as_vector(m_a)) )
{}
Square::Square( vect3d &pCenter,
vect3d &pNormal,
vect3d &pHeadVec,
float nWidth,
float nHeight,
float fReflR, float fRefrR, float fRefrK, float fEmitR)
:PrimaryObject(fReflR, fRefrR, fRefrK, fEmitR)
{
vecCopy(_vCenter, pCenter);
vecCopy(_vNormal, pNormal);
vecCopy(_vWidthVec, pHeadVec);
_nWidth = nWidth;
_nHeight = nHeight;
vecScale(_vWidthVec, nHeight/2, _v2WidthVec);
vect3d v2HeightVec;
cross_product(_vWidthVec, _vNormal, v2HeightVec);
normalize(v2HeightVec);
vecScale(v2HeightVec, nWidth/2, _v2HeightVec);
a = _vNormal[0]; b = _vNormal[1]; c = _vNormal[2];
d = (-1) * (a * _vCenter[0] + b * _vCenter[1] + c * _vCenter[2]);
updateBBox();
}
void Scene_c3t3_item::draw_triangle_edges(const Kernel::Point_3& pa,
const Kernel::Point_3& pb,
const Kernel::Point_3& pc)const {
#undef darker
Kernel::Vector_3 n = cross_product(pb - pa, pc - pa);
n = n / CGAL::sqrt(n*n);
positions_lines.push_back(pa.x());
positions_lines.push_back(pa.y());
positions_lines.push_back(pa.z());
positions_lines.push_back(pb.x());
positions_lines.push_back(pb.y());
positions_lines.push_back(pb.z());
positions_lines.push_back(pb.x());
positions_lines.push_back(pb.y());
positions_lines.push_back(pb.z());
positions_lines.push_back(pc.x());
positions_lines.push_back(pc.y());
positions_lines.push_back(pc.z());
positions_lines.push_back(pc.x());
positions_lines.push_back(pc.y());
positions_lines.push_back(pc.z());
positions_lines.push_back(pa.x());
positions_lines.push_back(pa.y());
positions_lines.push_back(pa.z());
}
