void randomizeInSphere(Particles& particles, real sphereSize)
{
const unsigned size = particles.size();
//const Real3& spherePos = sphere.p_;
particles.pos_.resize(size);
for (unsigned i=0;i<size;++i) {
particles.pos_[i]= sphereSize*( Real3(-.5f,-.5f,-.5f)+Real3(randnorm<real>(),randnorm<real>(),randnorm<real>()) );
// particles.pos_[i]= i%2?Real3(sphereSize-0.1,0,0):Real3(-sphereSize+0.1,0,0);
// particles.vel_[i]= i%2?Real3(2.5,0.5,0):Real3(-3,0,0);
// particles.acc_[i]=Real3(0.f,0.f,0.f);
}
}
MeshSurface::MeshSurface(const std::string filename, const Real3& edge_lengths)
: filename_(filename), edge_lengths_(edge_lengths)
{
#ifdef HAVE_VTK
{
reader_ = vtkSmartPointer<vtkSTLReader>::New();
reader_->SetFileName(filename_.c_str());
reader_->Update();
// tree_ = vtkSmartPointer<vtkOBBTree>::New();
// tree_->SetDataSet(reader_->GetOutput());
// tree_->BuildLocator();
}
{
Real bounds[6];
reader_->GetOutput()->GetBounds(bounds);
const Real xratio(edge_lengths_[0] / (bounds[1] - bounds[0]));
const Real yratio(edge_lengths_[1] / (bounds[3] - bounds[2]));
const Real zratio(edge_lengths_[2] / (bounds[5] - bounds[4]));
ratio_ = std::min(std::min(xratio, yratio), zratio);
shift_ = Real3(-bounds[0], -bounds[2], -bounds[4]); //XXX: align origin
}
#endif
}
void collideEachOtherQuad2(Particles& particles, real radius) {
Particles::Positions& pos = particles.pos_;
Particles::Velocities& dv = particles.dv_;
const unsigned size = pos.size();
Particles::Velocities dvic;
dvic.resize(pos.size(),Real3(0.));
// intercollision
real rad = radius;
real r = 2*rad;
for (unsigned i=0; i<size; ++i) {
Real3 pi = pos[i]+dv[i];
for (unsigned j=0; j<size; ++j) {
if (i!=j) {
Real3 pj = pos[j]+dv[j];
Real3 d = pi-pj;
real dsqn = d.sqrnorm();
if (dsqn < r*r) {
Real3 nd = normalize(d);
dvic[i]+=-d+nd*r;
}
}
}
}
for (unsigned i=0; i<size; ++i) {
if (dvic[i].sqrnorm()!=0) {
dv[i]+=dvic[i]*0.15;
}
}
}
void transformCoords() {
//rotModelX += lapse*0.001;
for (unsigned i=0;i<objs.size();++i) {
objs[i].rot_ = Rotation(rotModelX, Real3(0,1,0));
}
for (unsigned i=0;i<objs.size();++i) {
Matrix4 rmx = objs[i].rot_;
SetTranslation(rmx,objs[i].p_+Real3(0,offY,0));
for (unsigned j=0;j<objs[i].vx_.size();++j) {
objs[i].tvx_[j] =Matrix4Affine (rmx, objs[i].vx_[j]);
objs[i].tnormals_[j] = Matrix4Rotate(rmx,objs[i].normals_[j]);
}
}
}
void CompartmentSpaceVectorImpl::set_volume(const Real& volume)
{
if (volume <= 0)
{
throw std::invalid_argument("The volume must be positive.");
}
volume_ = volume;
const Real L(cbrt(volume));
edge_lengths_ = Real3(L, L, L);
}
virtual BDWorld* create_world(
const Real3& edge_lengths = Real3(1, 1, 1)) const
{
if (rng_)
{
return new BDWorld(edge_lengths, matrix_sizes_, rng_);
}
else
{
return new BDWorld(edge_lengths, matrix_sizes_);
}
}
void initModel(Object& obj, float data[][3], unsigned indices[][3], unsigned vsize, unsigned fsize,
const Color3& color,real scale) {
obj.colors_.reserve(vsize);
for (unsigned i=0;i<vsize;i++) {
obj.colors_.push_back(color);
}
obj.tvx_.resize(vsize);
obj.vx_.reserve(vsize);
for (unsigned i=0;i<vsize;i++) {
obj.vx_.push_back(scale*Real3(data[i][0],data[i][1],data[i][2]));
}
obj.faces_.reserve(fsize);
for (unsigned i=0;i<fsize;++i) {
obj.faces_.push_back(Int3(indices[i][0],indices[i][1],indices[i][2]));
}
obj.tnormals_.resize(vsize);
obj.normals_.reserve(vsize);
for (unsigned i = 0; i < vsize; i++) {
obj.normals_[i]+=Real3(0.);
}
for (unsigned i = 0; i < fsize; i++) {
unsigned ia=indices[i][0];
unsigned ib=indices[i][1];
unsigned ic=indices[i][2];
Real3 a = obj.vx_[ia];
Real3 b = obj.vx_[ib];
Real3 c = obj.vx_[ic];
Real3 n = normalize( crossProd( b-a, c-a) );
obj.normals_[ia]+=n;
obj.normals_[ib]+=n;
obj.normals_[ic]+=n;
}
for (unsigned i = 0; i < vsize; i++) {
obj.normals_[i]=normalize( obj.normals_[i] );
}
}
// http://www.cgafaq.info/wiki/Intersection_of_three_planes
bool threePlanesIntersection(const Plane& planeA, const Plane& planeB, const Plane& planeC, Real3& result)
{
Real3 bcCross = glm::cross(planeB.normal(), planeC.normal());
Real denom = glm::dot(planeA.normal(), bcCross);
if (denom == 0) {
result = Real3(0);
return false;
}
else {
result = (-planeA.distance() * bcCross
- planeB.distance() * glm::cross(planeC.normal(), planeA.normal())
- planeC.distance() * glm::cross(planeA.normal(), planeB.normal())) / denom;
return true;
}
}
virtual EGFRDWorld* create_world(
const Real3& edge_lengths = Real3(1, 1, 1)) const
{
if (rng_)
{
return new EGFRDWorld(edge_lengths, matrix_sizes_, rng_);
}
else if (matrix_sizes_[0] >= 3 && matrix_sizes_[1] >= 3
&& matrix_sizes_[2] >= 3)
{
return new EGFRDWorld(edge_lengths, matrix_sizes_);
}
else
{
return new EGFRDWorld(edge_lengths);
}
}
void collideLinearSweep (Particles& particles, real length) {
const real cubedivLength = 2*length/cubediv;
for (unsigned i=0;i<cubediv;++i) {
for (unsigned j=0;j<cubediv;++j) {
for (unsigned k=0;k<cubediv;++k) {
cube[i][j][k].clear();
}
}
}
Particles::Positions& pos = particles.pos_;
for (unsigned i=0;i<pos.size();++i) {
Real3 p = Real3(length,length,length) + pos[i];
int x = p[0]/cubedivLength;
int y = p[1]/cubedivLength;
int z = p[2]/cubedivLength;
cube[x][y][z].push_back(i);
}
}
std::vector<Real3> splitSphericalLineSegment(const Point& start, const Point& end, Real deltaAngle)
{
std::vector<Real3> result;
assert(start.position != -end.position);
auto direction = glm::normalize(glm::cross(start.position, end.position));
float distance = glm::acos(glm::dot(start.position, end.position));
result.push_back(start.position);
for (auto angle=deltaAngle; angle<distance; angle+=deltaAngle)
{
Mat4 rotation = glm::rotate(Mat4(1.0), angle, direction);
Real3 pos = glm::normalize(Real3(rotation * Real4(start.position, 1.0)));
result.push_back(pos);
}
result.push_back(end.position);
return result;
}
// http://tavianator.com/2011/05/fast-branchless-raybounding-box-intersections/
bool rayAabbIntersection(const Ray& ray, const AABB& aabb)
{
Real3 n_inv = Real3(1.0) / ray.direction();
double tx1 = (aabb.min().x - ray.origin().x)*n_inv.x;
double tx2 = (aabb.max().x - ray.origin().x)*n_inv.x;
double tmin = glm::min(tx1, tx2);
double tmax = glm::max(tx1, tx2);
double ty1 = (aabb.min().y - ray.origin().y)*n_inv.y;
double ty2 = (aabb.max().y - ray.origin().y)*n_inv.y;
tmin = glm::max(tmin, glm::min(ty1, ty2));
tmax = glm::min(tmax, glm::max(ty1, ty2));
double tz1 = (aabb.min().z - ray.origin().z)*n_inv.z;
double tz2 = (aabb.max().z - ray.origin().z)*n_inv.z;
tmin = glm::max(tmin, glm::min(tz1, tz2));
tmax = glm::min(tmax, glm::max(tz1, tz2));
return tmax >= glm::max(tmin, 0.0);
}
Real3 Point::tangent() const
{
CubeFaceBitSet bitSet;
Real3 binormal(1, 0, 0);
if (bitSet.test(CF_POSX))
{
binormal = Real3(0, 1, 0);
}
else if (bitSet.test(CF_NEGX))
{
binormal = Real3(0, 1, 0);
}
else if (bitSet.test(CF_POSY))
{
binormal = Real3(1, 0, 0);
}
else if (bitSet.test(CF_NEGY))
{
binormal = Real3(-1, 0, 0);
}
else if (bitSet.test(CF_POSZ))
{
binormal = Real3(0, 1, 0);
}
else if (bitSet.test(CF_NEGZ))
{
binormal = Real3(0, 1, 0);
}
else
{
assert(false);
}
Real3 normal = position;
Real3 result = glm::normalize(glm::cross(binormal, normal));
return result;
}
void init(unsigned w, unsigned h) {
ResourceMap resource;
resource.addPath("Fonts","");
std::string fontPath = resource.getPath("Fonts")+"monaco11";
font.load(fontPath.c_str());
console.reserve(height/font.lineHeight());
width=w;height=h;
objs.clear();
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
glFrustum (-1.0, 1.0, -1.0, 1.0, 1.5, 2000.0);
glEnable(GL_DEPTH_TEST);
glutIdleFunc (idle);
glutDisplayFunc (draw);
glutKeyboardFunc (keyboard);
//adding here the mouse processing callbacks
glutMotionFunc(processMouseActiveMotion);
glutPassiveMotionFunc(processMousePassiveMotion);
GLfloat mat_specular[] = { .5, .5, .5, 1.0 };
GLfloat mat_shininess[] = { 5.0 };
GLfloat light_position[] = { 1.0, 1.0, 1.0, 0.0 };
glClearColor (0.0, 0.0, 0.0, 0.0);
glShadeModel (GL_SMOOTH);
glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
glLightfv(GL_LIGHT0, GL_POSITION, light_position);
Color3 colors [] = {
Color3(255,0,0),
Color3(0,255,0),
Color3(0,0,255),
Color3(255,255,0),
Color3(0,255,255),
Color3(255,0,255),
Color3(255,255,255),
};
int n = 1;
for (unsigned i=0;i<n;i++) {
size_t size;
// size = objs.size();
// objs.push_back(Object());
// objs[size].p_ = DReal3(randnorm<dreal>(),randnorm<dreal>(),randnorm<dreal>());
// initModel(objs[objs.size()-1], icosa_vertex, icosa_indices,icosa_vertex_size,icosa_indices_size, colors[(i+5)%7],(7./n));
size = objs.size();
objs.push_back(Object());
// objs[size].p_ =0.8*DReal3(0.1+randnorm<dreal>(),0.1+randnorm<dreal>(),randnorm<dreal>());
//objs[size].p_ =0.5*DReal3(randnorm<dreal>(),randnorm<dreal>(),randnorm<dreal>());
objs[size].p_=Real3(0.5,0.,0.5)+0.1*Real3(randnorm<real>(),randnorm<real>(),randnorm<real>());
initModel(objs[objs.size()-1], bunny_vertex, bunny_indices,bunny_vertex_size,bunny_indices_size, colors[(i)%7],5/*(1./n)*/);
// initModel(objs[objs.size()-1], bunnyh_vertex, bunnyh_indices,bunnyh_vertex_size,bunnyh_indices_size, colors[(i)%7],5/*(1./n)*/);
}
gettimeofday(&idleold, 0);
transformCoords();
buildTree(objs,coltrees);
colres.resize(coltrees.size());
for (unsigned i=0;i<coltrees.size();i++) {
coltrees[i]->collideWithSegment(seg[0], seg[1], colres[i]);
}
}
virtual ODEWorld* create_world(
const Real3& edge_lengths = Real3(1, 1, 1)) const
{
return new ODEWorld(edge_lengths);
}
void Particles::update(const real& dt, const Real3& gravity, const Real3& shake) {
const unsigned size = pos_.size();
static Real3 oldG;
const real gdiffNorm = (oldG-gravity).norm();
const real timelapse = 0.3; //sec
static real cc = timelapse;
// dont overfeed accelerometer
if (gdiffNorm>0.05 || cc>=timelapse) {
cc=0;
for(unsigned i=0;i<size;++i) {
acc_[i]=gravity;
}
}
else {
for(unsigned i=0;i<size;++i) {
acc_[i]=Real3(0.);
}
cc+=dt;
}
oldG= gravity;
// drag (for small particles in viscous flows)
// f = bv where b is 6*pi*n*r
// n =fluid viscosity =10e-3 Pa*s as dynamic viscosity of water in SI
// r=10e-3 meters
// v = ((m*a)/b)*(1-eE(-(b*t)/m))
// const real n=1e-3f;
// const real r=10e-3f;
// const real mass = 1e-5f;
//
// real b = 6*Pi*n*r;
//
// for(unsigned i=0;i<size;++i) {
// Real3& v=vel_[i];
// Real3& a =acc_[i];
// Real3 dragForce = v*b;
// Real3 dragAcceleration = dragForce/mass;
// real dragNorm = dragAcceleration.norm();
// real aNorm = a.norm();
// a -= dragNorm < aNorm ? dragAcceleration : a;
// }
// weight over time these two (gravity and fluid acceleration)
// // fuid acceleration
// for(unsigned i=0;i<size;++i) {
// const Real3 fluid = normalize( Real3(-.5f,-.5f,-.5f) + Real3(randnorm<real>(),randnorm<real>(),randnorm<real>()) );
// acc_[i]+= shakelen*2.f*fluid;
// }
// gather acceleration & velocity deltas
for(unsigned i=0;i<size;++i) {
da_[i] = acc_[i]*dt;
dv_[i] = (vel_[i]+da_[i])*dt;
}
}
Real3 Get3Col(const Matrix4& A, int col) {
col = col*4;
return Real3(A[0+col],A[1+col],A[2+col]) ;
}
Real3 Get3Row(const Matrix4& A, int row) {
return Real3(A[0+row],A[4+row],A[8+row]) ;
}
void load_particle_space(const H5::Group& root, Tspace_* space)
{
typedef ParticleSpaceHDF5Traits traits_type;
typedef typename traits_type::h5_species_struct h5_species_struct;
typedef typename traits_type::h5_particle_struct h5_particle_struct;
Real3 edge_lengths;
const hsize_t dims[] = {3};
const H5::ArrayType lengths_type(H5::PredType::NATIVE_DOUBLE, 1, dims);
root.openAttribute("edge_lengths").read(lengths_type, &edge_lengths);
space->reset(edge_lengths);
double t;
root.openAttribute("t").read(H5::PredType::IEEE_F64LE, &t);
space->set_t(t);
{
H5::DataSet species_dset(root.openDataSet("species"));
const unsigned int num_species(
species_dset.getSpace().getSimpleExtentNpoints());
boost::scoped_array<h5_species_struct> h5_species_table(
new h5_species_struct[num_species]);
species_dset.read(
h5_species_table.get(), traits_type::get_species_comp_type());
species_dset.close();
H5::DataSet particle_dset(root.openDataSet("particles"));
const unsigned int num_particles(
particle_dset.getSpace().getSimpleExtentNpoints());
boost::scoped_array<h5_particle_struct> h5_particle_table(
new h5_particle_struct[num_particles]);
particle_dset.read(
h5_particle_table.get(), traits_type::get_particle_comp_type());
particle_dset.close();
typedef utils::get_mapper_mf<unsigned int, Species::serial_type>::type
species_id_map_type;
species_id_map_type species_id_map;
for (unsigned int i(0); i < num_species; ++i)
{
species_id_map[h5_species_table[i].id] = h5_species_table[i].serial;
}
for (unsigned int i(0); i < num_particles; ++i)
{
space->update_particle(ParticleID(std::make_pair(h5_particle_table[i].lot, h5_particle_table[i].serial)), Particle(Species(species_id_map[h5_particle_table[i].sid]), Real3(h5_particle_table[i].posx, h5_particle_table[i].posy, h5_particle_table[i].posz), h5_particle_table[i].radius, h5_particle_table[i].D));
}
// boost::scoped_array<h5_particle_struct>
// h5_particle_table(new h5_particle_struct[num_particles]);
// for (unsigned int i(0); i < num_particles; ++i)
// {
// species_id_map_type::const_iterator
// it(species_id_map.find(particles[i].second.species_serial()));
// if (it == species_id_map.end())
// {
// species.push_back(particles[i].second.species());
// it = species_id_map.insert(
// std::make_pair(particles[i].second.species_serial(),
// species.size())).first;
// }
// h5_particle_table[i].lot = particles[i].first.lot();
// h5_particle_table[i].serial = particles[i].first.serial();
// h5_particle_table[i].sid = (*it).second;
// h5_particle_table[i].posx = particles[i].second.position()[0];
// h5_particle_table[i].posy = particles[i].second.position()[1];
// h5_particle_table[i].posz = particles[i].second.position()[2];
// h5_particle_table[i].radius = particles[i].second.radius();
// h5_particle_table[i].D = particles[i].second.D();
// }
// boost::scoped_array<h5_species_struct>
// h5_species_table(new h5_species_struct[species.size()]);
// for (unsigned int i(0); i < species.size(); ++i)
// {
// h5_species_table[i].id = i + 1;
// std::strcpy(h5_species_table[i].serial,
// species[i].serial().c_str());
// }
}
}
void faceAxisDirection(ECubeFace face, Real3& s_dir, Real3& t_dir, Real3& p_dir)
{
switch (face)
{
case CF_POSX:
p_dir = Real3(1, 0, 0);
s_dir = Real3(0, 0, -1);
t_dir = Real3(0, 1, 0);
break;
case CF_NEGX:
p_dir = Real3(-1, 0, 0);
s_dir = Real3(0, 0, 1);
t_dir = Real3(0, 1, 0);
break;
case CF_POSY:
p_dir = Real3(0, 1, 0);
s_dir = Real3(0, 0, 1);
t_dir = Real3(1, 0, 0);
break;
case CF_NEGY:
p_dir = Real3(0, -1, 0);
s_dir = Real3(0, 0, 1);
t_dir = Real3(-1, 0, 0);
break;
case CF_POSZ:
p_dir = Real3(0, 0, 1);
s_dir = Real3(1, 0, 0);
t_dir = Real3(0, 1, 0);
break;
case CF_NEGZ:
p_dir = Real3(0, 0, -1);
s_dir = Real3(-1, 0, 0);
t_dir = Real3(0, 1, 0);
break;
default:
assert(0);
p_dir = Real3(1, 0, 0);
s_dir = Real3(0, 0, -1);
t_dir = Real3(0, 1, 0);
}
}