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