t_double mesh_element3d::get_dy () { plane_t plane1, plane2; get_plane(y_axis_minus, plane1); get_plane(y_axis_plus, plane2); t_double dy = 0.0; for (t_int i = 0; i < n_plane_corners; ++i) dy += plane1[i].y - plane2[i].y; return fabs (dy / n_plane_corners); }
t_double mesh_element3d::get_dz () { plane_t plane1, plane2; get_plane(z_axis_minus, plane1); get_plane(z_axis_plus, plane2); t_double dz = 0.0; for (t_int i = 0; i < n_plane_corners; ++i) dz += plane1[i].z - plane2[i].z; return fabs (dz / n_plane_corners); }
t_double mesh_element3d::get_dx () { plane_t plane1, plane2; get_plane(x_axis_minus, plane1); get_plane(x_axis_plus, plane2); t_double dx = 0.0; for (t_int i = 0; i < n_plane_corners; ++i) dx += plane1[i].x - plane2[i].x; return fabs (dx / n_plane_corners); }
bool Face3::intersects_aabb(const AABB &p_aabb) const { /** TEST PLANE **/ if (!p_aabb.intersects_plane(get_plane())) return false; #define TEST_AXIS(m_ax) \ /** TEST FACE AXIS */ \ { \ real_t aabb_min = p_aabb.position.m_ax; \ real_t aabb_max = p_aabb.position.m_ax + p_aabb.size.m_ax; \ real_t tri_min = vertex[0].m_ax; \ real_t tri_max = vertex[0].m_ax; \ for (int i = 1; i < 3; i++) { \ if (vertex[i].m_ax > tri_max) \ tri_max = vertex[i].m_ax; \ if (vertex[i].m_ax < tri_min) \ tri_min = vertex[i].m_ax; \ } \ \ if (tri_max < aabb_min || aabb_max < tri_min) \ return false; \ } TEST_AXIS(x); TEST_AXIS(y); TEST_AXIS(z); /** TEST ALL EDGES **/ Vector3 edge_norms[3] = { vertex[0] - vertex[1], vertex[1] - vertex[2], vertex[2] - vertex[0], }; for (int i = 0; i < 12; i++) { Vector3 from, to; p_aabb.get_edge(i, from, to); Vector3 e1 = from - to; for (int j = 0; j < 3; j++) { Vector3 e2 = edge_norms[j]; Vector3 axis = vec3_cross(e1, e2); if (axis.length_squared() < 0.0001) continue; // coplanar axis.normalize(); real_t minA, maxA, minB, maxB; p_aabb.project_range_in_plane(Plane(axis, 0), minA, maxA); project_range(axis, Transform(), minB, maxB); if (maxA < minB || maxB < minA) return false; } } return true; }
Face3::Side Face3::get_side_of(const Face3 &p_face, ClockDirection p_clock_dir) const { int over = 0, under = 0; Plane plane = get_plane(p_clock_dir); for (int i = 0; i < 3; i++) { const Vector3 &v = p_face.vertex[i]; if (plane.has_point(v)) //coplanar, don't bother continue; if (plane.is_point_over(v)) over++; else under++; } if (over > 0 && under == 0) return SIDE_OVER; else if (under > 0 && over == 0) return SIDE_UNDER; else if (under == 0 && over == 0) return SIDE_COPLANAR; else return SIDE_SPANNING; }
Vector<Vector3> PlaneShape::_gen_debug_mesh_lines() { Plane p = get_plane(); Vector<Vector3> points; Vector3 n1 = p.get_any_perpendicular_normal(); Vector3 n2 = p.normal.cross(n1).normalized(); Vector3 pface[4] = { p.normal * p.d + n1 * 10.0 + n2 * 10.0, p.normal * p.d + n1 * 10.0 + n2 * -10.0, p.normal * p.d + n1 * -10.0 + n2 * -10.0, p.normal * p.d + n1 * -10.0 + n2 * 10.0, }; points.push_back(pface[0]); points.push_back(pface[1]); points.push_back(pface[1]); points.push_back(pface[2]); points.push_back(pface[2]); points.push_back(pface[3]); points.push_back(pface[3]); points.push_back(pface[0]); points.push_back(p.normal * p.d); points.push_back(p.normal * p.d + p.normal * 3); return points; }
mesh_element3d::point3d_t mesh_element3d::get_dx_dy_dz () { plane_t plane1, plane2, plane3, plane4, plane5, plane6; get_plane(x_axis_minus, plane1); get_plane(x_axis_plus, plane2); get_plane(y_axis_minus, plane3); get_plane(y_axis_plus, plane4); get_plane(z_axis_minus, plane5); get_plane(z_axis_plus, plane6); point3d_t element_size; element_size[0] = 0; element_size[1] = 0; element_size[2] = 0; for (t_int i = 0; i < n_plane_corners; ++i) { element_size[0] += plane1[i].x - plane2[i].x; element_size[1] += plane3[i].y - plane4[i].y; element_size[2] += plane5[i].z - plane6[i].z; } element_size[0] = fabs (element_size[0] / n_plane_corners); element_size[1] = fabs (element_size[1] / n_plane_corners); element_size[2] = fabs (element_size[2] / n_plane_corners); return element_size; }
bool Pick( ik_pick_result &r, const ik_pick_query &q , CObject* ignore_object) { VERIFY( q.is_valid() ); float range = q.range(); collide::rq_result R; bool collided = false; Fvector pos = q.pos(); while( g_pGameLevel->ObjectSpace.RayPick( pos, q.dir(), range, collide::rqtBoth, R, ignore_object ) ) { Fvector next_pos = pos; float next_range = range; collided = get_plane( r, next_pos, next_range , R, range, pos, q.dir() ); if( collided ) break; range = next_range; pos = next_pos; if( range < EPS ) break; } #ifdef DEBUG if( ph_dbg_draw_mask1.test( phDbgDrawIKCollision ) && collided && !R.O ) { CDB::TRI *tri = Level( ).ObjectSpace.GetStaticTris( ) + R.element; Fvector p = q.pos();p.add( Fvector( ).mul( q.dir(), range ) ); DBG_DrawLine(pos,p,D3DCOLOR_XRGB( 255, 0, 0 ) ); if( tri ) { DBG_DrawTri( tri,Level( ).ObjectSpace.GetStaticVerts( ), D3DCOLOR_XRGB( 255, 0, 0 ) ); } } #endif return collided; }
void weighted_avg(Mesh& m) { std::vector<int>cnt(m.v.size()); std::vector<Vec3> projPos(m.v.size()); std::vector<Vec3>nbrPos(m.v.size()); std::vector<Plane> plane; avgNbrPos(m,nbrPos); m.get_normal_center(); get_plane(m,plane); //-------- // |disp|n // | | // \ | // \ | // \v| for(size_t ii=0;ii<m.t.size();ii++){ for(int jj=0;jj<3;jj++){ int vidx = m.t[ii][jj]; int label = m.t[ii].label; real_t d = plane[label].c.dot(plane[label].n); real_t disp=m.v[vidx].dot(plane[label].n); Vec3 vp=m.v[vidx]+plane[label].n*(d-disp); projPos[vidx]+=vp; cnt[vidx]++; } } for(size_t ii=0;ii<m.v.size();ii++){ projPos[ii]/=cnt[ii]; } float wSum=wN+wP+w0+wV; wSum=1/wSum; for(size_t ii=0;ii<m.v.size();ii++){ m.v[ii]=wSum*(wN*nbrPos[ii]+ wP*projPos[ii]+ wV*m.v[ii]+ w0*m.v0[ii]); } }
void Face3::get_support(const Vector3 &p_normal, const Transform &p_transform, Vector3 *p_vertices, int *p_count, int p_max) const { #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.98 #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.05 if (p_max <= 0) return; Vector3 n = p_transform.basis.xform_inv(p_normal); /** TEST FACE AS SUPPORT **/ if (get_plane().normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { *p_count = MIN(3, p_max); for (int i = 0; i < *p_count; i++) { p_vertices[i] = p_transform.xform(vertex[i]); } return; } /** FIND SUPPORT VERTEX **/ int vert_support_idx = -1; real_t support_max = 0; for (int i = 0; i < 3; i++) { real_t d = n.dot(vertex[i]); if (i == 0 || d > support_max) { support_max = d; vert_support_idx = i; } } /** TEST EDGES AS SUPPORT **/ for (int i = 0; i < 3; i++) { if (i != vert_support_idx && i + 1 != vert_support_idx) continue; // check if edge is valid as a support real_t dot = (vertex[i] - vertex[(i + 1) % 3]).normalized().dot(n); dot = ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { *p_count = MIN(2, p_max); for (int j = 0; j < *p_count; j++) p_vertices[j] = p_transform.xform(vertex[(j + i) % 3]); return; } } *p_count = 1; p_vertices[0] = p_transform.xform(vertex[vert_support_idx]); }
t_double mesh_element3d::calc_volume() { t_double volume = 0.0; //share for 12 tetraidr (6 side -> 2 tetraidr for each) /* volume = 0; fpoint3d_t center = get_center (); volume += calc_tetra_volume(corners[0],corners[1],corners[2], center); volume += calc_tetra_volume(corners[1],corners[2],corners[3], center); volume += calc_tetra_volume(corners[1],corners[3],corners[7], center); volume += calc_tetra_volume(corners[1],corners[5],corners[7], center); volume += calc_tetra_volume(corners[0],corners[2],corners[6], center); volume += calc_tetra_volume(corners[0],corners[4],corners[6], center); volume += calc_tetra_volume(corners[4],corners[5],corners[6], center); volume += calc_tetra_volume(corners[5],corners[6],corners[7], center); volume += calc_tetra_volume(corners[0],corners[1],corners[4], center); volume += calc_tetra_volume(corners[1],corners[4],corners[5], center); volume += calc_tetra_volume(corners[2],corners[3],corners[6], center); volume += calc_tetra_volume(corners[3],corners[6],corners[7], center); */ /* share for 5 tetraidr volume += calc_tetra_volume(corners[0],corners[4],corners[5], corners[6]); volume += calc_tetra_volume(corners[0],corners[5],corners[1], corners[3]); volume += calc_tetra_volume(corners[0],corners[3],corners[2], corners[6]); volume += calc_tetra_volume(corners[5],corners[7],corners[6], corners[3]); volume += calc_tetra_volume(corners[0],corners[3],corners[5], corners[6]); */ /* share for 6 tetraidr volume += calc_tetra_volume(corners[0],corners[1],corners[3], corners[7]); volume += calc_tetra_volume(corners[0],corners[1],corners[5], corners[7]); volume += calc_tetra_volume(corners[0],corners[4],corners[5], corners[7]); volume += calc_tetra_volume(corners[0],corners[2],corners[3], corners[7]); volume += calc_tetra_volume(corners[0],corners[2],corners[6], corners[7]); volume += calc_tetra_volume(corners[0],corners[4],corners[6], corners[7]); */ fpoint3d_t centerx1, centerx2; fpoint3d_t centery1, centery2; fpoint3d_t centerz1, centerz2; plane_t plane; get_plane(x_axis_minus, plane); centerx1 = plane[0] + plane[1] + plane[2] + plane[3]; //get_plane_center (plane, centerx1); get_plane(x_axis_plus, plane); //get_plane_center (plane, centerx2); centerx2 = plane[0] + plane[1] + plane[2] + plane[3]; get_plane(y_axis_minus, plane); //get_plane_center (plane, centery1); centery1 = plane[0] + plane[1] + plane[2] + plane[3]; get_plane(y_axis_plus, plane); //get_plane_center (plane, centery2); centery2 = plane[0] + plane[1] + plane[2] + plane[3]; get_plane(z_axis_minus, plane); //get_plane_center (plane, centerz1); centerz1 = plane[0] + plane[1] + plane[2] + plane[3]; get_plane(z_axis_plus, plane); //get_plane_center (plane, centerz2); centerz2 = plane[0] + plane[1] + plane[2] + plane[3]; volume = fpoint3d_mixed_multiplicate (centerx1 - centerx2, centery1 - centery2,centerz1 - centerz2); return fabs(volume * 0.015625); // 0.25*0.25*0.25 }
//static int iter=0; void cgd(Mesh & m) { int height=11*m.t.size()+6*m.v.size(); // int height=2*m.t.size()+3*m.v.size(); // int width = 3*(m.v.size()); int width = 3*(m.v.size()+m.t.size()); //nvar=m.v.size(); std::cout << "compute mesh info\n"; nvar=m.v.size()+m.t.size(); std::vector<Plane>plane; get_plane(m,plane); add_v4(m); std::vector<Mat3>mat; vmat(m,mat); m.adjlist(); CCS ccs; std::vector<double >b; std::cout << "compute matrix rows info\n"; //b for smoothness matrix is 0 //smooth_mat(m,mat,wS,ccs); vert_smooth_mat(m,wS,ccs,b); identity_mat(m, mat, wI, ccs , b); vertex_mat(m,wV0,ccs,b); planar_mat(m,plane,wPt,ccs); b.resize(height,0.0); SparseCCS* s = new SparseCCS(width, height, &ccs.vals[0], &ccs.rowInds[0], &ccs.colPtr[0]); SparseCCS* st = s->transposed(); std::cout << "compute AA^T\n"; SparseCCS* sst=s->multiply_LM(*st); SparseMatrixOutline *outline = new SparseMatrixOutline(width); //printAB(ccs,b,0); double * ATb= s->operator* (&b[0]); // int rowcnt=sst->rows_count(); int colcnt=sst->columns_count(); const real * vals=sst->values(); const int * row_indices=sst->row_indices(); const int * col_pointers=sst->column_pointers(); for(int ii=0; ii<colcnt; ii++) { for(int jj=col_pointers[ii]; jj<col_pointers[ii+1]; jj++) { outline->AddEntry(row_indices[jj],ii,vals[jj]); } } // delete s; delete st; delete sst; std::cout << "matrix assembly\n"; SparseMatrix A(outline); delete outline; std::cout << "lin solve\n"; CGSolver solver(&A); double * x=new double [width]; vertex2arr(m,x); double eps = 1E-6; int maxIter = 50; int verbose = 0; int ret = solver.SolveLinearSystemWithJacobiPreconditioner(x, ATb, eps, maxIter, verbose);// //if(ret<0) { // printf("optimization error\n"); // } A.CheckLinearSystemSolution(x,ATb); printf("\n"); array2vertex(x,m); delete []x; delete []ATb; }