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()); }