void AABox::minmax_sq_dist(const CGLA::Vec3f& p,
float& dmin, float& dmax) const
{
const Vec3f a = 0.5*pmax-0.5*pmin;
const Vec3f p0 = pmin + a;
Vec3f d = p-p0;
Vec3f f(d);
for(int i=0;i<3;++i)
{
if(f[i]>=0)
f[i] = p[i]-pmin[i];
else
f[i] = p[i]-pmax[i];
if(d[i]<-a[i])
d[i] = p[i]-pmin[i];
else if(d[i]>a[i])
d[i] = p[i]-pmax[i];
else
d[i] = 0;
}
dmin = sqr_length(d);
// dmax = sqr_length(p-interior_point);
dmax = sqr_length(f);
assert(dmin<=dmax);
}
bool VisObj::select_vertex(const CGLA::Vec2i& pos)
{
float d;
if(depth_pick(pos[0], pos[1], d))
{
Vec3d c;
float r;
bsphere(mani, c, r);
VertexID closest = InvalidVertexID;
double min_dist = DBL_MAX;
for(auto vid : mani.vertices())
{
Vec3d wp = world2screen(mani.pos(vid));
if(sqr_length(Vec2d(wp[0],wp[1])-Vec2d(pos))<100)
{
double dist = sqr_length(screen2world(pos[0], pos[1], d)-mani.pos(vid));
if(dist < min_dist)
{
min_dist = dist;
closest = vid;
}
}
}
if(closest != InvalidVertexID) {
vertex_selection.resize(mani.allocated_vertices(),0);
vertex_selection[closest] = !vertex_selection[closest];
active_selection = true;
post_create_display_list();
return true;
}
}
return false;
}
inline void normalize() {
T sl = sqr_length();
if ( sl > 0 ) {
T inv_len = 1. / sqrt( sl );
a *= inv_len;
b *= inv_len;
c *= inv_len;
}
}
fn Vec3
project(Vec3 a, Vec3 b)
{
Vec3 r = a * dot(a, b);
f32 l = sqr_length(r);
if(l < 1.0f)
{
r /= l;
}
return r;
}
std::set<NodeKey> Query::neighborhood(vec3 from, double max_distance) {
std::set<NodeKey> res;
double max_distanceSqr = max_distance * max_distance;
for (auto & nodeiter : mesh->nodes()){
vec3 to_pos = nodeiter.get_pos();
double lengthSqr = sqr_length(from - to_pos);
if (lengthSqr < max_distanceSqr){
res.insert(nodeiter.key());
}
}
return res;
}
OBox OBox::box_triangle(const Triangle& t)
{
Vec3f e0 = t.get_v1()-t.get_v0();
Vec3f e1 = t.get_v2()-t.get_v1();
Vec3f e2 = t.get_v0()-t.get_v2();
Vec3f X,Y,Z;
if(sqr_length(e0) > sqr_length(e1))
{
if(sqr_length(e0) > sqr_length(e2))
{
X = normalize(e0);
Y = normalize(e1 - X * dot(X, e1));
}
else
{
X = normalize(e2);
Y = normalize(e0 - X * dot(X, e0));
}
}
else
{
if(sqr_length(e1) > sqr_length(e2))
{
X = normalize(e1);
Y = normalize(e2 - X * dot(X, e2));
}
else
{
X = normalize(e2);
Y = normalize(e0 - X * dot(X, e0));
}
}
Z = cross(X,Y);
const Mat3x3f Rot(X,Y,Z);
Vec3f p0 = Rot * t.get_v0();
Vec3f p1 = Rot * t.get_v1();
Vec3f p2 = Rot * t.get_v2();
Vec3f pmin = v_min(p0, v_min(p1, p2));
Vec3f pmax = v_max(p0, v_max(p1, p2));
Vec3f centre_close = v_max(pmin, v_min(pmax, Rot * t.get_centre()));
return OBox(Rot, AABox(pmin, pmax, centre_close));
}
static void sim_Sigma(SEXP da){
SEXP V = GET_SLOT(da, install("Sigma")) ;
int *dm = DIMS_SLOT(da), *Gp = Gp_SLOT(da),
*nc = NCOL_SLOT(da), *nlev = NLEV_SLOT(da);
int nT = dm[nT_POS], mc = imax(nc, nT);
double *v, su, *u = U_SLOT(da),
*scl = Alloca(mc * mc, double);
R_CheckStack();
for (int i = 0; i < nT; i++){
v = REAL(VECTOR_ELT(V, i));
if (nc[i] == 1){ /* simulate from the inverse-Gamma */
su = sqr_length(u + Gp[i], nlev[i]);
v[0] = 1/rgamma(0.5 * nlev[i] + IG_SHAPE, 1.0/(su * 0.5 + IG_SCALE));
}
else { /* simulate from the inverse-Wishart */
mult_xtx(nlev[i], nc[i], u + Gp[i], scl); /* t(x) * (x) */
for (int j = 0; j < nc[i]; j++) scl[j * j] += 1.0; /* add prior (identity) scale matrix */
solve_po(nc[i], scl, v);
rwishart(nc[i], (double) (nlev[i] + nc[i]), v, scl);
solve_po(nc[i], scl, v);
}
}
}
AABox AABox::box_and_split(const std::vector<Triangle>& invec,
std::vector<Triangle>& lvec,
std::vector<Triangle>& rvec)
{
const size_t N = invec.size();
Vec3f tri_pmin(FLT_MAX), tri_pmax(-FLT_MAX);
for(size_t i=0;i<N;++i)
{
tri_pmin = v_min(invec[i].get_pmin(), tri_pmin);
tri_pmax = v_max(invec[i].get_pmax(), tri_pmax);
}
Vec3f diff = tri_pmax - tri_pmin;
// Find the point closest to the centre.
Vec3f centre = tri_pmin + diff;
Vec3f centre_close = invec[0].get_v0();
float min_dist = FLT_MAX;
for(size_t i=0;i<N;++i)
{
Vec3f v0 = invec[i].get_v0();
Vec3f v1 = invec[i].get_v1();
Vec3f v2 = invec[i].get_v2();
float sl0 = sqr_length(centre-v0);
if(sl0 < min_dist)
{
min_dist = sl0;
centre_close = v0;
}
float sl1 = sqr_length(centre-v1);
if(sl1 < min_dist)
{
min_dist = sl1;
centre_close = v1;
}
float sl2 = sqr_length(centre-v2);
if(sl2 < min_dist)
{
min_dist = sl2;
centre_close = v2;
}
}
int k;
if(diff[0]>diff[1])
{
if(diff[0]>diff[2])
k = 0;
else
k = 2;
}
else
{
if(diff[1]>diff[2])
k = 1;
else
k = 2;
}
float thresh = diff[k]/2.0f + tri_pmin[k];
for(size_t i=0;i<N;++i)
{
if(invec[i].get_centre()[k] > thresh)
rvec.push_back(invec[i]);
else
lvec.push_back(invec[i]);
}
if(lvec.empty() || rvec.empty())
{
lvec.clear();
lvec.insert(lvec.end(),
invec.begin(),
invec.begin()+N/2);
rvec.clear();
rvec.insert(rvec.end(),
invec.begin()+N/2,
invec.end());
}
assert(!lvec.empty());
assert(!rvec.empty());
assert(lvec.size()+rvec.size() == invec.size());
return AABox(tri_pmin, tri_pmax, centre_close);
}
OBox OBox::box_and_split(const std::vector<Triangle>& invec,
std::vector<Triangle>& lvec,
std::vector<Triangle>& rvec)
{
// Obtain the rotation matrix for the OBB
const Mat3x3f Rot = compute_rotation(invec);
const int N_tri = invec.size();
const int N_pts = 3*N_tri;
// Compute the rotated set of points and the extents of the point aligned
// BBox.
vector<Vec3f> pts(N_pts);
Vec3f tri_pmin(FLT_MAX), tri_pmax(-FLT_MAX);
for(int i=0;i<N_tri;++i)
{
const Triangle& tri = invec[i];
int offs = 3*i;
pts[offs ] = Rot*tri.get_v0();
pts[offs+1] = Rot*tri.get_v1();
pts[offs+2] = Rot*tri.get_v2();
for(int j=0;j<3;++j)
{
tri_pmin = v_min(pts[offs+j], tri_pmin);
tri_pmax = v_max(pts[offs+j], tri_pmax);
}
}
// Find the point closest to the centre.
const Vec3f centre = tri_pmin + 0.5f*(tri_pmax-tri_pmin);
Vec3f centre_close;
float min_dist = FLT_MAX;
for(int i=0;i<N_pts;++i)
{
Vec3f v = pts[i];
float sl = sqr_length(centre-v);
if(sl < min_dist)
{
min_dist = sl;
centre_close = v;
}
}
// Partition the triangles
const float thresh = centre[0];
for(int i=0;i<N_tri;++i)
{
Vec3f p = Rot * invec[i].get_centre();
if( p[0] > thresh)
rvec.push_back(invec[i]);
else
lvec.push_back(invec[i]);
}
// If all triangles landed in one box, split them naively.
if(lvec.empty() || rvec.empty())
{
lvec.clear();
lvec.insert(lvec.end(),
invec.begin(),
invec.begin()+N_tri/2);
rvec.clear();
rvec.insert(rvec.end(),
invec.begin()+N_tri/2,
invec.end());
}
return OBox(Rot, AABox(tri_pmin, tri_pmax, centre_close));
}
int process(
const tParticle* aParticles,
const Vec3f& queryPos,
const tFunc& aFunc) const
{
const Vec3f distMin = queryPos - mBBoxMin;
const Vec3f distMax = mBBoxMax - queryPos;
for(int i=0; i<3; i++)
{
if(distMin[i] < 0.f || distMax[i] < 0.f) {
return -1;
}
}
const Vec3f cellPt = mInvCellSize * distMin;
const Vec3f coordF(
std::floor(cellPt.x),
std::floor(cellPt.y),
std::floor(cellPt.z));
const int px = int(coordF.x);
const int py = int(coordF.y);
const int pz = int(coordF.z);
const Vec3f fractCoord = cellPt - coordF;
const int pxo = px + (fractCoord.x < 0.5f ? -1 : +1);
const int pyo = py + (fractCoord.y < 0.5f ? -1 : +1);
const int pzo = pz + (fractCoord.z < 0.5f ? -1 : +1);
int found = 0;
for(int j=0; j<8; j++)
{
Vec2i activeRange;
switch(j)
{
case 0: activeRange = GetCellRange(GetCellIndex(Vec3i(px , py , pz ))); break;
case 1: activeRange = GetCellRange(GetCellIndex(Vec3i(px , py , pzo))); break;
case 2: activeRange = GetCellRange(GetCellIndex(Vec3i(px , pyo, pz ))); break;
case 3: activeRange = GetCellRange(GetCellIndex(Vec3i(px , pyo, pzo))); break;
case 4: activeRange = GetCellRange(GetCellIndex(Vec3i(pxo, py , pz ))); break;
case 5: activeRange = GetCellRange(GetCellIndex(Vec3i(pxo, py , pzo))); break;
case 6: activeRange = GetCellRange(GetCellIndex(Vec3i(pxo, pyo, pz ))); break;
case 7: activeRange = GetCellRange(GetCellIndex(Vec3i(pxo, pyo, pzo))); break;
}
for(; activeRange.x < activeRange.y; activeRange.x++)
{
const int particleIndex = mIndices[activeRange.x];
const tParticle &particle = aParticles[particleIndex];
const float distSqr =
sqr_length(queryPos - getPosition(particle));
if(distSqr <= mRadiusSqr) {
aFunc(particle);
++found;
}
}
}
return found;
}
inline T length() const { return sqrt( sqr_length() ); }
