void cloth::bilateral(phys::system& system) {
using namespace math;
core::each( boost::edges(mesh), [&](const mesh_type::edge_descriptor& e) {
natural i = boost::source(e, mesh);
natural j = boost::target(e, mesh);
auto info = mesh[e];
const vec3 diff = dofs[i].g() - dofs[j].g();
if( diff.norm() < 1e-5 ) core::log("ALARRRM");
const vec3 n = diff.normalized();
auto& J = system.constraint.bilateral.matrix;
real norm = 2 * std::sqrt( 2 * diff.squaredNorm() );
real scale = 1/norm;
J(info.key, &dofs[i]) = scale * 2 * diff.transpose(); // n.transpose();
J(info.key, &dofs[j]) = -2 * scale * diff.transpose(); // - n.transpose();
auto& b = system.constraint.bilateral.values;
b(info.key).setZero();
auto& c = system.constraint.bilateral.corrections;
c(info.key).setConstant( scale * (info.rest * info.rest - diff.squaredNorm()) );
});
}
mat44 mat44::look_at(const vec3& eye, const vec3& look, const vec3& up)
{
vec3 forward = (look - eye).normalize();
vec3 real_up = (up - forward * dot(forward, up.normalized())).normalize();
vec3 right = cross(real_up, forward).normalize();
mat44 rst(
vec4(right, -dot(eye, right)),
vec4(real_up, -dot(eye, real_up)),
vec4(forward, -dot(eye, forward)),
vec4(0, 0, 0, 1)
);
return rst;
}
/**
* @brief LookAt Setup view matrix
* @param eye Camera position
* @param center Looking center
* @param up Up vector
* @return
*/
mat4 LookAt(vec3 eye, vec3 view, vec3 up)
{
vec3 z_cam = view.normalized();
vec3 x_cam = up.cross(z_cam).normalized();
vec3 y_cam = z_cam.cross(x_cam);
mat3 R = mat3::Zero();
R.col(0) = x_cam;
R.col(1) = y_cam;
R.col(2) = z_cam;
mat4 L = mat4::Zero();
L.block(0, 0, 3, 3) = R.transpose();
L(3, 3) = 1.0f;
L.block(0, 3, 3, 1) = -R.transpose() * (eye);
return L;
}
void gep::CameraLookAtHorizon::setViewVector(const vec3& vector)
{
m_viewDir = vector.normalized();
}
void gep::CameraLookAtHorizon::setUpVector(const vec3& vector)
{
m_upVector = vector.normalized();
}
void graph::reset(math::natural n) {
width = n;
height = n;
obj.resize( width * height);
constraint.mesh = mesh_type( width * height );
auto pos = [&](natural i) -> vec3& {
return obj[i].conf;
};
auto vel = [&](natural i) -> vec3& {
return obj[i].velocity;
};
auto edge = [&](real rest) -> edge_type {
edge_type res;
res.rest = rest;
res.constraint = new phys::constraint::bilateral(1);
return res;
};
// place vertices
for(natural i = 0 ; i < n * n; ++i ) {
const natural x = i / width;
const natural y = i % width;
pos(i) = (sceneRadius()/2) * vec3(y, 0, x) / width;
vel(i).setZero();
}
// create edges
for(natural i = 0 ; i < n * n; ++i ) {
const natural x = i / width;
const natural y = i % width;
bool ok_height = (x < height - 1);
bool ok_width = (y < width - 1);
if( ok_height ) {
real rest = (pos(i) - pos(i + width)).norm();
boost::add_edge(i, i + width, edge(rest), constraint.mesh);
}
if( ok_width ) {
real rest = (pos(i) - pos(i + 1)).norm();
boost::add_edge(i, i + 1, edge(rest), constraint.mesh);
}
if( ok_height && ok_width ) {
// real rest = (pos(i) - pos(i + width + 1)).norm();
// boost::add_edge(i, i + width + 1, edge(rest), constraint.mesh);
// rest = (pos(i + 1) - pos(i + width)).norm();
// boost::add_edge(i + 1, i + width, edge(rest), constraint.mesh);
}
}
frame[0]->transform( SE3::translation( pos(0) ) );
frame[1]->transform( SE3::translation( pos(width - 1) ) );
system.clear();
// masses
core::each( obj, [&](const obj_type& o) {
system.mass[ &o ].setIdentity(3, 3);
system.resp[ &o ].setIdentity(3, 3);
});
constraint.lambda.clear();
constraint.update.clear();
// setup constraints
core::each( boost::edges(constraint.mesh), [&](const mesh_type::edge_descriptor& e) {
auto c = constraint.mesh[e].constraint;
natural i = boost::source(e, constraint.mesh);
natural j = boost::target(e, constraint.mesh);
const real rest = constraint.mesh[e].rest;
constraint.update[c] = [&,i,j,c,e,rest] {
const vec3 diff = obj[i].conf - obj[j].conf;
const vec3 n = diff.normalized();
auto& row = system.constraint.bilateral.matrix[ c ];
row[ &obj[i] ] = n.transpose();
row[ &obj[j] ] = -n.transpose();
const real gamma = 1;
system.constraint.bilateral.values[ c ] = vec1::Zero();
system.constraint.bilateral.corrections[ c ] = gamma * vec1::Constant( (rest - diff.norm()) );
if( solver ) {
constraint.mesh[e].acyclic =
solver->acyclic().id.constraint.find(c) != solver->acyclic().id.constraint.end();
}
};
});
constraint.fixed[ frame[0] ].dof = 0;
constraint.fixed[ frame[1] ].dof = width - 1;
core::each(constraint.fixed, [&](gui::frame* f, const constraint_type::attach& a) {
// constraint.update[a.key] = [&,a,f]{
// const vec3 diff = f->transform().translation() - obj[a.dof].conf;
// // TODO only once
// system.constraint.bilateral.matrix[a.key][&obj[a.dof].dof] = mat33::Identity();
// system.constraint.bilateral.values[a.key] = vec3::Zero();
// system.constraint.bilateral.corrections[a.key] = diff;
// };
});
core::each(constraint.update, [&](phys::constraint::bilateral::key,
const core::callback& u) {
u();
});
// setup solver
solver.reset( new solver_type(system) );
// display mesh
display.mesh.clear();
for(natural i = 0 ; i < n * n; ++i ) {
const natural x = i / width;
const natural y = i % width;
bool ok_height = (x < height - 1);
bool ok_width = (y < width - 1);
if( ok_height && ok_width ) {
display.mesh.quads.push_back(geo::mesh::quad{i, i + 1, i + width + 1, i + width } );
}
};
};
//! Set position and direction of the ray. If \a normalize is true, \a a_direction will be normalized.
void setValue(const vec3<Type>& a_origin, const vec3<Type>& a_direction, bool normalize = true)
{
m_origin = a_origin;
m_direction = normalize ? a_direction.normalized() : a_direction;
}
