Vec3r rotateVector(const Matrix44r& mat, const Vec3r& v) {
Vec3r res;
for (unsigned i = 0; i < 3; i++) {
res[i] = 0;
for (unsigned j = 0; j < 3; j++)
res[i] += mat(i, j) * v[j];
}
res.normalize();
return res;
}
bool Line::intersect(const SphereVolume &sphere,
Real &enter,
Real &exit ) const
{
Vec3r v;
Pnt3r center;
sphere.getCenter(center);
Real radius;
Real h;
Real b;
Real d;
Real t1;
Real t2;
radius = sphere.getRadius();
v = center - _pos;
h = (v.dot(v))-(radius * radius);
b = (v.dot(_dir));
if(h >= 0.f && b <= 0.f)
return false;
d = b * b - h;
if(d < 0.f)
return false;
d = osgSqrt(d);
t1 = b - d;
// if (t1 > 1)
// return false;
t2 = b + d;
if( t1 < TypeTraits<Real>::getDefaultEps() )
{
if( t2 < TypeTraits<Real>::getDefaultEps() /*|| t2 > 1*/)
{
return false;
}
}
enter = t1;
exit = t2;
return true;
}
void FEdgeXDetector::preProcessFace(WXFace *iFace){
Vec3r firstPoint = iFace->GetVertex(0)->GetVertex();
Vec3r N = iFace->GetNormal();
// Compute the dot product between V (=_Viewpoint - firstPoint) and N:
Vec3r V(_Viewpoint - firstPoint);
N.normalize();
V.normalize();
iFace->SetDotP(N * V);
// compute the distance between the face center and the viewpoint:
Vec3r dist_vec(iFace->center() - _Viewpoint);
iFace->SetZ(dist_vec.norm());
}
inline static real angle(WOEdge *h)
{
const Vec3r& n1 = h->GetbFace()->GetNormal();
const Vec3r& n2 = h->GetaFace()->GetNormal();
const Vec3r v = h->GetVec();
real sine = (n1 ^ n2) * v / v.norm();
if (sine >= 1.0) {
return M_PI / 2.0;
}
if (sine <= -1.0) {
return -M_PI / 2.0;
}
return ::asin(sine);
}
void FEdgeXDetector::ProcessSilhouetteFace(WXFace *iFace, bool meshSilhouettes)
{
real NdotVepsilonHack = 0;// 0.05; //0.1; //0.01; //0.1;
// SILHOUETTE LAYER
// Compute the dot products between View direction and N at each vertex
// of the face:
Vec3r point;
int closestPointId = 0;
real dist, minDist = FLT_MAX;
int numVertices = iFace->numberOfVertices();
WXFaceLayer * faceLayer = new WXFaceLayer(iFace, Nature::SILHOUETTE, true);
Vec3r normal;
if(meshSilhouettes){
// Use per face normal
normal = (iFace->GetVertex(2)->GetVertex() - iFace->GetVertex(0)->GetVertex()) ^ (iFace->GetVertex(1)->GetVertex() - iFace->GetVertex(0)->GetVertex());
normal.normalize();
}
for(int i=0; i<numVertices; i++){
point = iFace->GetVertex(i)->GetVertex();
if(!meshSilhouettes){
// Use per vertex normal
normal = iFace->GetVertexNormal(i);
normal.normalize();
}
Vec3r V(_Viewpoint - point);
V.normalize();
real d = normal * V + NdotVepsilonHack;
faceLayer->PushDotP(d);
// Find the point the closest to the viewpoint
Vec3r dist_vec(point - _Viewpoint);
dist = dist_vec.norm();
if(dist < minDist) {
minDist = dist;
closestPointId = i;
}
// store ndotv at the vertex for use in the region-based visibility
// assert(dynamic_cast<WXVertex*>(iFace->GetVertex(i))!=NULL);
((WXVertex*)iFace->GetVertex(i))->setNdotV(d);
}
// Set the closest point id:
faceLayer->SetClosestPointIndex(closestPointId);
// Add this layer to the face:
iFace->AddSmoothLayer(faceLayer);
}
static real angle_from_cotan(WVertex *vo, WVertex *v1, WVertex *v2)
{
/* cf. Appendix B and the caption of Table 1 from [Meyer et al 2002] */
real udotv, denom;
Vec3r u (v1->GetVertex() - vo->GetVertex());
Vec3r v(v2->GetVertex() - vo->GetVertex());
udotv = u * v;
denom = sqrt(u.squareNorm() * v.squareNorm() - udotv * udotv);
/* Note: I assume this is what they mean by using atan2(). -Ray Jones */
/* tan = denom/udotv = y/x (see man page for atan2) */
return (fabs(atan2(denom, udotv)));
}
void MonaghanKernel::monaghanGradient( const Vec3r& r, Vec3r& gradient )
{
HReal dist = r.length();
HReal q = dist*m_invH;
gradient.fill(0.0);
if( q >= 0 && q < 1 )
{
HReal scalar = -3.0f*(2-q)*(2-q);
scalar += 12.0f*(1-q)*(1-q);
gradient = (m_g*m_invH*scalar/dist)*r;
}
else if ( q >=1 && q < 2 )
{
HReal scalar = -3.0f*(2-q)*(2-q);
gradient = (m_g*scalar*m_invH/dist)*r;
}
}
void Grid::castInfiniteRay(const Vec3r& orig,
const Vec3r& dir,
OccludersSet& occluders,
unsigned timestamp) {
Vec3r end = Vec3r(orig + FLT_MAX * dir / dir.norm());
bool inter = initInfiniteRay(orig, dir, timestamp);
if(!inter)
return;
allOccludersGridVisitor visitor(occluders);
castRayInternal(visitor);
}
// precondition1: P is inside the sphere
// precondition2: P,V points to the outside of the sphere (i.e. OP.V > 0)
static bool sphere_clip_vector(const Vec3r& O, real r, const Vec3r& P, Vec3r& V)
{
Vec3r W = P - O;
real a = V.squareNorm();
real b = 2.0 * V * W;
real c = W.squareNorm() - r * r;
real delta = b * b - 4 * a * c;
if (delta < 0) {
// Should not happen, but happens sometimes (numerical precision)
return true;
}
real t = - b + ::sqrt(delta) / (2.0 * a);
if (t < 0.0) {
// Should not happen, but happens sometimes (numerical precision)
return true;
}
if (t >= 1.0) {
// Inside the sphere
return false;
}
V[0] = (t * V.x());
V[1] = (t * V.y());
V[2] = (t * V.z());
return true;
}
void FEdgeXDetector::preProcessFace(WXFace *iFace)
{
Vec3r firstPoint = iFace->GetVertex(0)->GetVertex();
Vec3r N = iFace->GetNormal();
// Compute the dot product between V (=_Viewpoint - firstPoint) and N:
Vec3r V;
if (_orthographicProjection) {
V = Vec3r(0.0, 0.0, _Viewpoint.z() - firstPoint.z());
}
else {
V = Vec3r(_Viewpoint - firstPoint);
}
N.normalize();
V.normalize();
iFace->setDotP(N * V);
// compute the distance between the face center and the viewpoint:
if (_orthographicProjection) {
iFace->setZ(iFace->center().z() - _Viewpoint.z());
}
else {
Vec3r dist_vec(iFace->center() - _Viewpoint);
iFace->setZ(dist_vec.norm());
}
}
void VelocityMotor::internal_update()
{
// check if we have a solid
if (mData.solid == NULL || isEnabled() == false) return ;
Vec3r targetVelocity = mData.velocity;
Solid * solid = mData.solid;
Vec3r currentAchievedVelocity = solid->getGlobalLinearVel();
if (doesGravityAffectSolid())
{
Vec3r gravity = mSimulator->getGravity();
if (gravity.length() > 0)
{
Vec3r gravity_velocity = project(gravity, currentAchievedVelocity);
currentAchievedVelocity -= gravity_velocity;
}
}
Vec3r deltaVelocity = targetVelocity - currentAchievedVelocity;
Vec3r forceVector = deltaVelocity / mSimulator->getStepSize() * solid->getMass();
if (!doesGravityAffectSolid())
forceVector -= mSimulator->getGravity() * solid->getMass();
if (forceVector.length() > getMaximumForce())
{
forceVector.normalize();
forceVector *= getMaximumForce();
}
Force controllingForce;
controllingForce.duration = 0;
controllingForce.singleStep = true;
controllingForce.type = GLOBAL_FORCE;
controllingForce.vec = forceVector;
solid->addForce(controllingForce);
}
void ThirdPersonCamera::LookAt(const Vec3r &eye, const Vec3r &target, const Vec3r &up)
{
m_eye = eye;
m_target = target;
m_targetYAxis = up;
m_zAxis = eye - target;
m_zAxis.Normalise();
m_viewDir = m_zAxis * -1;
m_xAxis = Vec3r::CrossProduct(up, m_zAxis);
m_xAxis.Normalise();
m_yAxis = Vec3r::CrossProduct(m_zAxis, m_xAxis);
m_yAxis.Normalise();
// m_xAxis.Normalise();
m_viewMatrix.Access(0,0) = m_xAxis.X;
m_viewMatrix.Access(1,0) = m_xAxis.Y;
m_viewMatrix.Access(2,0) = m_xAxis.Z;
m_viewMatrix.Access(3,0) = -Vec3r::DotProduct(m_xAxis, eye);
m_viewMatrix.Access(0,1) = m_yAxis.X;
m_viewMatrix.Access(1,1) = m_yAxis.Y;
m_viewMatrix.Access(2,1) = m_yAxis.Z;
m_viewMatrix.Access(3,1) = -Vec3r::DotProduct(m_yAxis, eye);
m_viewMatrix.Access(0,2) = m_zAxis.X;
m_viewMatrix.Access(1,2) = m_zAxis.Y;
m_viewMatrix.Access(2,2) = m_zAxis.Z;
m_viewMatrix.Access(3,2) = -Vec3r::DotProduct(m_zAxis, eye);
m_orientation.FromMatrix(m_viewMatrix.m_matrix);
Vec3r offset = m_target - m_eye;
m_offsetDistance = offset.Length();
}
HReal MonaghanKernel::monaghanValue( const Vec3r & r )
{
HReal value = 0.0;
HReal q = r.length()*m_invH;
if( q >= 0 && q < 1 )
{
value = m_v*( (2-q)*(2-q)*(2-q) - 4.0f*(1-q)*(1-q)*(1-q));
}
else if ( q >=1 && q < 2 )
{
value = m_v*( (2-q)*(2-q)*(2-q) );
}
else
{
value = 0.0f;
}
return value;
}
void FEdgeXDetector::ProcessSilhouetteFace(WXFace *iFace)
{
// SILHOUETTE LAYER
Vec3r normal;
// Compute the dot products between View direction and N at each vertex of the face:
Vec3r point;
int closestPointId = 0;
real dist, minDist = FLT_MAX;
int numVertices = iFace->numberOfVertices();
WXFaceLayer *faceLayer = new WXFaceLayer(iFace, Nature::SILHOUETTE, true);
for (int i = 0; i < numVertices; i++) {
point = iFace->GetVertex(i)->GetVertex();
normal = iFace->GetVertexNormal(i);
normal.normalize();
Vec3r V;
if (_orthographicProjection) {
V = Vec3r(0.0, 0.0, _Viewpoint.z() - point.z());
}
else {
V = Vec3r(_Viewpoint - point);
}
V.normalize();
real d = normal * V;
faceLayer->PushDotP(d);
// Find the point the closest to the viewpoint
if (_orthographicProjection) {
dist = point.z() - _Viewpoint.z();
}
else {
Vec3r dist_vec(point - _Viewpoint);
dist = dist_vec.norm();
}
if (dist < minDist) {
minDist = dist;
closestPointId = i;
}
}
// Set the closest point id:
faceLayer->setClosestPointIndex(closestPointId);
// Add this layer to the face:
iFace->AddSmoothLayer(faceLayer);
}
real CurvePoint::curvature2d_as_angle() const
{
#if 0
Vec3r edgeA = (_FEdges[0])->orientation2d();
Vec3r edgeB = (_FEdges[1])->orientation2d();
Vec2d N1(-edgeA.y(), edgeA.x());
N1.normalize();
Vec2d N2(-edgeB.y(), edgeB.x());
N2.normalize();
return acos((N1 * N2));
#endif
if (__A == 0)
return __B->curvature2d_as_angle();
if (__B == 0)
return __A->curvature2d_as_angle();
return ((1 - _t2d) * __A->curvature2d_as_angle() + _t2d * __B->curvature2d_as_angle());
}
bool intersectRayBBox(const Vec3r& orig, const Vec3r& dir, // ray origin and direction
const Vec3r& boxMin, const Vec3r& boxMax, // the bbox
real t0, real t1,
real& tmin, real& tmax, // I0=orig+tmin*dir is the first intersection, I1=orig+tmax*dir is the second intersection
real epsilon){
float tymin, tymax, tzmin, tzmax;
Vec3r inv_direction(1.0/dir[0], 1.0/dir[1], 1.0/dir[2]);
int sign[3];
sign[0] = (inv_direction.x() < 0);
sign[1] = (inv_direction.y() < 0);
sign[2] = (inv_direction.z() < 0);
Vec3r bounds[2];
bounds[0] = boxMin;
bounds[1] = boxMax;
tmin = (bounds[sign[0]].x() - orig.x()) * inv_direction.x();
tmax = (bounds[1-sign[0]].x() - orig.x()) * inv_direction.x();
tymin = (bounds[sign[1]].y() - orig.y()) * inv_direction.y();
tymax = (bounds[1-sign[1]].y() - orig.y()) * inv_direction.y();
if ( (tmin > tymax) || (tymin > tmax) )
return false;
if (tymin > tmin)
tmin = tymin;
if (tymax < tmax)
tmax = tymax;
tzmin = (bounds[sign[2]].z() - orig.z()) * inv_direction.z();
tzmax = (bounds[1-sign[2]].z() - orig.z()) * inv_direction.z();
if ( (tmin > tzmax) || (tzmin > tmax) )
return false;
if (tzmin > tmin)
tmin = tzmin;
if (tzmax < tmax)
tmax = tzmax;
return ( (tmin < t1) && (tmax > t0) );
}
/**
* @brief 円柱を設定
* @param [in] R Controlクラスのポインタ
* @param [in] pos_x 中心座標(x,y) （無次元）
* @param [in] pos_y 中心座標(x,y) （無次元）
* @param [in] radius 半径 （無次元）
* @param [in] len_z 円柱のZ方向の長さ （無次元）
* @param [in] mid_solid 固体媒質ID
* @param [out] cut カット情報
* @param [out] bid 境界ID
*/
void IP_Cylinder::setCircle(Control* R,
const REAL_TYPE pos_x,
const REAL_TYPE pos_y,
const REAL_TYPE radius,
const REAL_TYPE len_z,
const int mid_solid,
long long* cut,
int* bid)
{
Vec3r pch(pitch); ///< 無次元格子幅
Vec3r org(origin); ///< ノードローカル基点座標の無次元値
REAL_TYPE dx = pitch[0];
REAL_TYPE dy = pitch[1];
REAL_TYPE dz = pitch[2];
REAL_TYPE cx = pos_x; ///< 円柱の中心座標（無次元）
REAL_TYPE cy = pos_y; ///< 円柱の中心座標（無次元）
REAL_TYPE rs = radius; ///< 円柱の半径（無次元）
int mid_s = mid_solid;
// 球のbbox
Vec3r box_min; ///< Bounding boxの最小値
Vec3r box_max; ///< Bounding boxの最大値
Vec3i box_st; ///< Bounding boxの始点インデクス
Vec3i box_ed; ///< Bounding boxの終点インデクス
box_min.assign( -(REAL_TYPE)rs+cx, -(REAL_TYPE)rs+cy, 0.0 ); // Z方向はダミー
box_max.assign( (REAL_TYPE)rs+cx, (REAL_TYPE)rs+cy, 0.0 );
box_st = find_index(box_min, org, pch);
box_ed = find_index(box_max, org, pch);
int ix = size[0];
int jx = size[1];
int kx = size[2];
int gd = guide;
// ローカルな無次元基点座標
REAL_TYPE ox = origin[0];
REAL_TYPE oy = origin[1];
REAL_TYPE oz = origin[2];
// グローバルな無次元座標
REAL_TYPE zs = oz;
REAL_TYPE ze;
if ( mode == dim_2d )
{
ze = oz + region[2] + dz; // Z方向には全セルを対象, dzは安全係数
}
else
{
ze = oz + len_z; // Z-面からの距離
}
// カット情報
Vec3r p[5];
Vec3r base; // セルのシフト量
Vec3r b; // セルセンタ座標
REAL_TYPE lb[5]; // 内外判定フラグ
for (int k=1; k<=kx; k++) {
for (int j=box_st.y-1; j<=box_ed.y+1; j++) {
for (int i=box_st.x-1; i<=box_ed.x+1; i++) {
REAL_TYPE z = org.z + 0.5*dz + dz*(REAL_TYPE)(k-1);
if ( z <= ze )
{
base.assign((REAL_TYPE)i-0.5, (REAL_TYPE)j-0.5, (REAL_TYPE)k-0.5);
b = org + base*pch;
p[0].assign(b.x , b.y , b.z ); // p
p[1].assign(b.x-dx, b.y , b.z ); // w
p[2].assign(b.x+dx, b.y , b.z ); // e
p[3].assign(b.x , b.y-dy, b.z ); // s
p[4].assign(b.x , b.y+dy, b.z ); // n
// (cx, cy, *)が球の中心
for (int l=0; l<5; l++) {
REAL_TYPE x = p[l].x - cx;
REAL_TYPE y = p[l].y - cy;
REAL_TYPE rr = sqrt(x*x + y*y);
lb[l] = ( rr <= rs ) ? -1.0 : 1.0; // 内側がマイナス
}
// cut test 注意！ インデクスが1-4
for (int l=1; l<=4; l++)
{
if ( lb[0]*lb[l] < 0.0 )
{
REAL_TYPE s = cut_line_2d(p[0], l, rs);
size_t m = _F_IDX_S3D(i, j, k, ix, jx, kx, gd);
int r = quantize9(s);
setCut9(cut[m], r, l-1);
setBit5(bid[m], mid_s, l-1);
int rr = quantize9(1.0-s);
size_t m1;
switch (l-1) {
case X_minus:
m1 = _F_IDX_S3D(i-1, j, k, ix, jx, kx, gd);
setBit5(bid[m1], mid_s, X_plus);
setCut9(cut[m1], rr, X_plus);
break;
case X_plus:
m1 = _F_IDX_S3D(i+1, j, k, ix, jx, kx, gd);
setBit5(bid[m1], mid_s, X_minus);
setCut9(cut[m1], rr, X_minus);
break;
case Y_minus:
m1 = _F_IDX_S3D(i, j-1, k, ix, jx, kx, gd);
setBit5(bid[m1], mid_s, Y_plus);
setCut9(cut[m1], rr, Y_plus);
break;
case Y_plus:
m1 = _F_IDX_S3D(i, j+1, k, ix, jx, kx, gd);
setBit5(bid[m1], mid_s, Y_minus);
setCut9(cut[m1], rr, Y_minus);
break;
}
}
}
} // if z branch
}
}
}
if ( mode == dim_2d ) return;
// Z+方向
#pragma omp parallel for firstprivate(ix, jx, kx, gd, mid_s, ox, oy, oz, dx, dy, dz, ze, rs, cx, cy) schedule(static)
for (int k=1; k<=kx; k++) {
for (int j=1; j<=jx; j++) {
for (int i=1; i<=ix; i++) {
size_t m = _F_IDX_S3D(i, j, k, ix, jx, kx, gd);
REAL_TYPE x = ox + 0.5*dx + dx*(i-1); // position of cell center
REAL_TYPE y = oy + 0.5*dy + dy*(j-1);
REAL_TYPE z = oz + 0.5*dz + dz*(k-1);
REAL_TYPE s = (ze - z)/dz;
REAL_TYPE xx = x - cx;
REAL_TYPE yy = y - cy;
if ( (xx*xx + yy*yy) <= rs*rs )
{
if ( (z <= ze) && (ze < z+dz) )
{
setBit5(bid[m], mid_s, Z_plus);
int r = quantize9(s);
setCut9(cut[m], r, Z_plus);
size_t m1 = _F_IDX_S3D(i, j, k+1, ix, jx, kx, gd);
setBit5(bid[m1], mid_s, Z_minus);
int rr = quantize9(1.0-s);
setCut9(cut[m1], rr, Z_minus);
}
else if ( (z-dz < ze) && (ze < z) )
{
setBit5(bid[m], mid_s, Z_minus);
int r = quantize9(-s);
setCut9(cut[m], r, Z_minus);
size_t m1 = _F_IDX_S3D(i, j, k-1, ix, jx, kx, gd);
setBit5(bid[m1], mid_s, Z_plus);
int rr = quantize9(1.0+s);
setCut9(cut[m1], rr, Z_plus);
}
}
}
}
}
// Z-方向
#pragma omp parallel for firstprivate(ix, jx, kx, gd, mid_s, ox, oy, oz, dx, dy, dz, zs, rs, cx, cy) schedule(static)
for (int k=1; k<=kx; k++) {
for (int j=1; j<=jx; j++) {
for (int i=1; i<=ix; i++) {
size_t m = _F_IDX_S3D(i, j, k, ix, jx, kx, gd);
REAL_TYPE x = ox + 0.5*dx + dx*(i-1); // position of cell center
REAL_TYPE y = oy + 0.5*dy + dy*(j-1);
REAL_TYPE z = oz + 0.5*dz + dz*(k-1);
REAL_TYPE s = (zs - z)/dz;
REAL_TYPE xx = x - cx;
REAL_TYPE yy = y - cy;
if ( (xx*xx + yy*yy) <= rs*rs )
{
if ( (z <= zs) && (zs < z+dz) )
{
setBit5(bid[m], mid_s, Z_plus);
int r = quantize9(s);
setCut9(cut[m], r, Z_plus);
size_t m1 = _F_IDX_S3D(i, j, k+1, ix, jx, kx, gd);
setBit5(bid[m1], mid_s, Z_minus);
int rr = quantize9(1.0-s);
setCut9(cut[m1], rr, Z_minus);
}
else if ( (z-dz < zs) && (zs < z) )
{
setBit5(bid[m], mid_s, Z_minus);
int r = quantize9(-s);
setCut9(cut[m], r, Z_minus);
size_t m1 = _F_IDX_S3D(i, j, k-1, ix, jx, kx, gd);
setBit5(bid[m1], mid_s, Z_plus);
int rr = quantize9(1.0+s);
setCut9(cut[m1], rr, Z_plus);
}
}
}
}
}
}
bool Line::intersect(const CylinderVolume &cyl,
Real &enter,
Real &exit ) const
{
Real radius = cyl.getRadius();
Vec3r adir;
Vec3r o_adir;
Pnt3r apos;
cyl.getAxis(apos, adir);
o_adir = adir;
adir.normalize();
bool isect;
Real ln;
Real dl;
Vec3r RC;
Vec3r n;
Vec3r D;
RC = _pos - apos;
n = _dir.cross (adir);
ln = n .length( );
if(ln == 0.f) // IntersectionLine is parallel to CylinderAxis
{
D = RC - (RC.dot(adir)) * adir;
dl = D.length();
if(dl <= radius) // line lies in cylinder
{
enter = 0.f;
exit = Inf;
}
else
{
return false;
}
}
else
{
n.normalize();
dl = osgAbs(RC.dot(n)); //shortest distance
isect = (dl <= radius);
if(isect)
{ // if ray hits cylinder
Real t;
Real s;
Vec3r O;
O = RC.cross(adir);
t = - (O.dot(n)) / ln;
O = n.cross(adir);
O.normalize();
s = osgAbs (
(osgSqrt ((radius * radius) - (dl * dl))) / (_dir.dot(O)));
exit = t + s;
if(exit < 0.f)
return false;
enter = t - s;
if(enter < 0.f)
enter = 0.f;
}
else
{
return false;
}
}
Real t;
Plane bottom(-adir, apos);
if(bottom.intersect(*this, t))
{
if(bottom.isInHalfSpace(_pos))
{
if(t > enter)
enter = t;
}
else
{
if(t < exit)
exit = t;
}
}
else
{
if(bottom.isInHalfSpace(_pos))
return false;
}
Plane top(adir, apos + o_adir);
if(top.intersect(*this, t))
{
if(top.isInHalfSpace(_pos))
{
if(t > enter)
enter = t;
}
else
{
if(t < exit)
exit = t;
}
}
else
{
if(top.isInHalfSpace(_pos))
return false;
}
return (enter < exit);
}
/*! Intersect the line with a triangle.
\param [in] v0,v1,v2 Points definiting a triangle in CW orientation.
\param [out] t If hit, this returns the distance the hit is down the line.
\param [in,out] norm If non-NULL, this is set to the normal at the point of
intersection
\returns True if there is an intersection.
This algorithm is based on "Fast, Minimum Storage Ray/Triangle Instersection" by
T. Moeller and B. Trumbore, with the addition of avoiding the computation of
inv_det when no intersection happens.
*/
bool Line::intersect(const Pnt3r &v0,
const Pnt3r &v1,
const Pnt3r &v2,
Real &t,
Vec3r *norm) const
{
// Eps (1E-6f) didn't work with very small geometries!
static const Real sEps = 1E-10f;
// find vectors for two edges sharing v0.
Vec3r edge1 = v1 - v0;
Vec3r edge2 = v2 - v0;
// begin calculating determinant - also used to calculate U parameter.
Vec3r pvec = _dir.cross(edge2);
// if determinant is near zero, ray lies in plane of triangle.
Real det = edge1.dot(pvec);
Vec3r qvec;
if(det > sEps)
{
// calculate distance from v0 to ray origin.
Vec3r tvec = _pos - v0;
// calculate U parameter and test bounds.
Real u = tvec.dot(pvec);
if(u < 0.f || u > det)
{
return false;
}
// prepare to test V parameter.
qvec = tvec.cross(edge1);
// calculate V parameter and test bounds.
Real v = _dir.dot(qvec);
if(v < 0.f || u + v > det)
{
return false;
}
}
else if(det < -sEps)
{
// calculate distance from v0 to ray origin.
Vec3r tvec = _pos - v0;
// calculate U parameter and test bounds.
Real u = tvec.dot(pvec);
if(u > 0.f || u < det)
{
return false;
}
// prepare to test V parameter.
qvec = tvec.cross(edge1);
// calculate V parameter and test bounds.
Real v = _dir.dot(qvec);
if(v > 0.f || u + v < det)
{
return false;
}
}
else
{
return false; // ray is parallel to the plane of the triangle.
}
Real inv_det = 1.0f / det;
// calculate t, ray intersects triangle.
t = edge2.dot(qvec) * inv_det;
if(norm != NULL)
{
*norm = edge1.cross(edge2);
norm->normalize();
}
return true;
}
// assume the point is inside the triangle
Vec3r ComputeBarycentricCoords(const Vec3r A,const Vec3r B,const Vec3r C,const Vec3r P)
{
Vec3r BA = B-A;
Vec3r CA = C-A;
Vec3r N = BA^CA;
Vec3r PA = A-P;
Vec3r PB = B-P;
Vec3r PC = C-P;
double areaA = (PB^PC) * N;
double areaB = (PC^PA) * N;
double areaC = (PA^PB) * N;
#if 0
// checking
printf("Barycentric debug: ----------------------------------\n");
printf("A = [%f %f %f], B = [%f %f %f], C = [%f %f %f]\n",
A.x(), A.y(), A.z(), B.x(), B.y(), B.z(), C.x(), C.y(), C.z());
printf("P = [%f %f %f]\n", P.x(), P.y(), P.z());
printf("areas = [%f %f %f]\n", areaA, areaB, areaC);
printf("plot3([%f %f %f %f],[%f %f %f %f],[%f %f %f %f],'r-');\n",
A.x(), B.x(), C.x(), A.x(),
A.y(), B.y(), C.y(), A.y(),
A.z(), B.z(), C.z(), A.z());
printf("hold on; plot3(%f,%f,%f,'bo');\n",
P.x(),P.y(),P.z());
#endif
assert(areaA > -0.1 && areaB > -0.1 && areaC > -0.1);
if (areaA < 0)
areaA = 0;
if (areaB < 0)
areaB = 0;
if (areaC < 0)
areaC = 0;
double totalArea = areaA + areaB + areaC;
double a = areaA / totalArea;
double b = areaB / totalArea;
double c = areaC / totalArea;
// printf("c = [%f %f %f]\n", a,b,c);
Vec3r result(a,b,c);
return result;
}
bool FEdge::intersectParametric(FEdge & fe2, Vec3r viewpoint, real t3D, real u3D)
{
Vec3r A1 = vertexA()->getPoint3D();
Vec3r B1 = vertexB()->getPoint3D();
Vec3r A2 = fe2.vertexA()->getPoint3D();
Vec3r B2 = fe2.vertexB()->getPoint3D();
if (sameSide(A1,B1,viewpoint, A2, B2) || sameSide(A2, B2, viewpoint, A1, B1))
return false;
// now, there *must* be an intersection.
// for each edge, the normal of the plane containing the edge and the viewpoint
Vec3r N1 = (A1-viewpoint) ^ (B1-viewpoint);
Vec3r N2 = (A2-viewpoint) ^ (B2-viewpoint);
// direction vector of the intersection of the two planes.
Vec3r V = N1 ^ N2;
// check if the planes coincide (i.e., source edges are colinear)
assert(V.norm() > 0);
// ----- compute t parameter ------
// form a plane for line 1, normal to the plane containing the viewpoint
Vec3r BA1 = B1 - A1;
Vec3r hsNormal1 = N1 ^ BA1;
// intersect ray in direction of V through the plane
real w1;
GeomUtils::intersection_test res1 = GeomUtils::intersectLinePlanePN(viewpoint, V, hsNormal1, A1, w1);
if (res1 != GeomUtils::DO_INTERSECT)
{
printf("res1 = %d\n", res1);
printf("viewpoint = [%f %f %f]\n", viewpoint[0], viewpoint[1], viewpoint[2]);
printf("A1 = [%f %f %f]\n", A1[0], A1[1], A1[2]);
printf("B1 = [%f %f %f]\n", B1[0], B1[1], B1[2]);
printf("A2 = [%f %f %f]\n", A2[0], A2[1], A2[2]);
printf("B2 = [%f %f %f]\n", B2[0], B2[1], B2[2]);
printf("N1 = [%f %f %f]\n", N1[0], N1[1], N1[2]);
printf("N2 = [%f %f %f]\n", N2[0], N2[1], N2[2]);
printf("V = [%f %f %f]\n", V[0], V[1], V[2]);
printf("hsNormal1 = [%f %f %f]\n", hsNormal1[0], hsNormal1[1], hsNormal1[2]);
}
assert(res1 == GeomUtils::DO_INTERSECT);
Vec3r pt1 = viewpoint + w1 * V;
t3D = ((pt1 - A1) * BA1) / (BA1*BA1);
assert(t3D >=0 && t3D <= 1);
// if (t3D < 0 || t3D > 1)
// return false;
// ----- compute u parameter ------
// form a half-space plane for line 2
Vec3r BA2 = B2 - A2;
Vec3r hsNormal2 = N2 ^ BA2;
real w2;
GeomUtils::intersection_test res2 = GeomUtils::intersectLinePlanePN(viewpoint, V, hsNormal2, A2, w2);
if (res2 != GeomUtils::DO_INTERSECT)
{
printf("res1 = %d\n", res1);
printf("viewpoint = [%f %f %f]\n", viewpoint[0], viewpoint[1], viewpoint[2]);
printf("A1 = [%f %f %f]\n", A1[0], A1[1], A1[2]);
printf("B1 = [%f %f %f]\n", B1[0], B1[1], B1[2]);
printf("A2 = [%f %f %f]\n", A2[0], A2[1], A2[2]);
printf("B2 = [%f %f %f]\n", B2[0], B2[1], B2[2]);
printf("N1 = [%f %f %f]\n", N1[0], N1[1], N1[2]);
printf("N2 = [%f %f %f]\n", N2[0], N2[1], N2[2]);
printf("V = [%f %f %f]\n", V[0], V[1], V[2]);
printf("hsNormal2 = [%f %f %f]\n", hsNormal2[0], hsNormal2[1], hsNormal2[2]);
}
assert(res2 == GeomUtils::DO_INTERSECT);
Vec3r pt2 = viewpoint + w2 * V;
u3D = ((pt2 - A2) * BA2) / (BA2*BA2);
assert( u3D >=0 && u3D <=1);
// if (u3D < 0 || u3D > 1)
// return false;
return true;
}
Vec2r SilhouetteGeomEngine::WorldToImage2(const Vec3r & M)
{
Vec3r newPoint = WorldToImage(M);
return Vec2r(newPoint.x(), newPoint.y());
}
FEdge *ViewEdgeXBuilder::BuildSmoothFEdge(FEdge *feprevious, const OWXFaceLayer& ifl)
{
WOEdge *woea, *woeb;
real ta, tb;
SVertex *va, *vb;
FEdgeSmooth *fe;
// retrieve exact silhouette data
WXSmoothEdge *se = ifl.fl->getSmoothEdge();
if (ifl.order) {
woea = se->woea();
woeb = se->woeb();
ta = se->ta();
tb = se->tb();
}
else {
woea = se->woeb();
woeb = se->woea();
ta = se->tb();
tb = se->ta();
}
Vec3r normal;
// Make the 2 Svertices
if (feprevious == 0) { // that means that we don't have any vertex already built for that face
Vec3r A1(woea->GetaVertex()->GetVertex());
Vec3r A2(woea->GetbVertex()->GetVertex());
Vec3r A(A1 + ta * (A2 - A1));
va = MakeSVertex(A, false);
// Set normal:
Vec3r NA1(ifl.fl->getFace()->GetVertexNormal(woea->GetaVertex()));
Vec3r NA2(ifl.fl->getFace()->GetVertexNormal(woea->GetbVertex()));
Vec3r na((1 - ta) * NA1 + ta * NA2);
na.normalize();
va->AddNormal(na);
normal = na;
// Set CurvatureInfo
CurvatureInfo *curvature_info_a =
new CurvatureInfo(*(dynamic_cast<WXVertex*>(woea->GetaVertex())->curvatures()),
*(dynamic_cast<WXVertex*>(woea->GetbVertex())->curvatures()), ta);
va->setCurvatureInfo(curvature_info_a);
}
else {
va = feprevious->vertexB();
}
Vec3r B1(woeb->GetaVertex()->GetVertex());
Vec3r B2(woeb->GetbVertex()->GetVertex());
Vec3r B(B1 + tb * (B2 - B1));
if (feprevious && (B - va->point3D()).norm() < 1.0e-6)
return feprevious;
vb = MakeSVertex(B, false);
// Set normal:
Vec3r NB1(ifl.fl->getFace()->GetVertexNormal(woeb->GetaVertex()));
Vec3r NB2(ifl.fl->getFace()->GetVertexNormal(woeb->GetbVertex()));
Vec3r nb((1 - tb) * NB1 + tb * NB2);
nb.normalize();
normal += nb;
vb->AddNormal(nb);
// Set CurvatureInfo
CurvatureInfo *curvature_info_b =
new CurvatureInfo(*(dynamic_cast<WXVertex*>(woeb->GetaVertex())->curvatures()),
*(dynamic_cast<WXVertex*>(woeb->GetbVertex())->curvatures()), tb);
vb->setCurvatureInfo(curvature_info_b);
// Creates the corresponding feature edge
fe = new FEdgeSmooth(va, vb);
fe->setNature(ifl.fl->nature());
fe->setId(_currentFId);
fe->setFrsMaterialIndex(ifl.fl->getFace()->frs_materialIndex());
fe->setFace(ifl.fl->getFace());
fe->setFaceMark(ifl.fl->getFace()->GetMark());
if (feprevious == 0)
normal.normalize();
fe->setNormal(normal);
fe->setPreviousEdge(feprevious);
if (feprevious)
feprevious->setNextEdge(fe);
_pCurrentSShape->AddEdge(fe);
va->AddFEdge(fe);
vb->AddFEdge(fe);
++_currentFId;
ifl.fl->userdata = fe;
return fe;
}
bool inBox(const Vec3r& inter, const Vec3r& box_min, const Vec3r& box_max){
if(((inter.x()>=box_min.x()) && (inter.x() <box_max.x()))
&& ((inter.y()>=box_min.y()) && (inter.y() <box_max.y()))
&& ((inter.z()>=box_min.z()) && (inter.z() <box_max.z()))
){
return true;
}
return false;
}
/*! gts_vertex_principal_directions:
* @v: a #WVertex.
* @s: a #GtsSurface.
* @Kh: mean curvature normal (a #Vec3r).
* @Kg: Gaussian curvature (a real).
* @e1: first principal curvature direction (direction of largest curvature).
* @e2: second principal curvature direction.
*
* Computes the principal curvature directions at a point given @Kh and @Kg, the mean curvature normal and
* Gaussian curvatures at that point, computed with gts_vertex_mean_curvature_normal() and
* gts_vertex_gaussian_curvature(), respectively.
*
* Note that this computation is very approximate and tends to be unstable. Smoothing of the surface or the principal
* directions may be necessary to achieve reasonable results.
*/
void gts_vertex_principal_directions(WVertex *v, Vec3r Kh, real Kg, Vec3r &e1, Vec3r &e2)
{
Vec3r N;
real normKh;
Vec3r basis1, basis2, d, eig;
real ve2, vdotN;
real aterm_da, bterm_da, cterm_da, const_da;
real aterm_db, bterm_db, cterm_db, const_db;
real a, b, c;
real K1, K2;
real *weights, *kappas, *d1s, *d2s;
int edge_count;
real err_e1, err_e2;
int e;
WVertex::incoming_edge_iterator itE;
/* compute unit normal */
normKh = Kh.norm();
if (normKh > 0.0) {
Kh.normalize();
}
else {
/* This vertex is a point of zero mean curvature (flat or saddle point). Compute a normal by averaging
* the adjacent triangles
*/
N[0] = N[1] = N[2] = 0.0;
for (itE = v->incoming_edges_begin(); itE != v->incoming_edges_end(); itE++)
N = Vec3r(N + (*itE)->GetaFace()->GetNormal());
real normN = N.norm();
if (normN <= 0.0)
return;
N.normalize();
}
/* construct a basis from N: */
/* set basis1 to any component not the largest of N */
basis1[0] = basis1[1] = basis1[2] = 0.0;
if (fabs (N[0]) > fabs (N[1]))
basis1[1] = 1.0;
else
basis1[0] = 1.0;
/* make basis2 orthogonal to N */
basis2 = (N ^ basis1);
basis2.normalize();
/* make basis1 orthogonal to N and basis2 */
basis1 = (N ^ basis2);
basis1.normalize();
aterm_da = bterm_da = cterm_da = const_da = 0.0;
aterm_db = bterm_db = cterm_db = const_db = 0.0;
int nb_edges = v->GetEdges().size();
weights = (real *)malloc(sizeof(real) * nb_edges);
kappas = (real *)malloc(sizeof(real) * nb_edges);
d1s = (real *)malloc(sizeof(real) * nb_edges);
d2s = (real *)malloc(sizeof(real) * nb_edges);
edge_count = 0;
for (itE = v->incoming_edges_begin(); itE != v->incoming_edges_end(); itE++) {
WOEdge *e;
WFace *f1, *f2;
real weight, kappa, d1, d2;
Vec3r vec_edge;
if (!*itE)
continue;
e = *itE;
/* since this vertex passed the tests in gts_vertex_mean_curvature_normal(), this should be true. */
//g_assert(gts_edge_face_number (e, s) == 2);
/* identify the two triangles bordering e in s */
f1 = e->GetaFace();
f2 = e->GetbFace();
/* We are solving for the values of the curvature tensor
* B = [ a b ; b c ].
* The computations here are from section 5 of [Meyer et al 2002].
*
* The first step is to calculate the linear equations governing the values of (a,b,c). These can be computed
* by setting the derivatives of the error E to zero (section 5.3).
*
* Since a + c = norm(Kh), we only compute the linear equations for dE/da and dE/db. (NB: [Meyer et al 2002]
* has the equation a + b = norm(Kh), but I'm almost positive this is incorrect).
*
* Note that the w_ij (defined in section 5.2) are all scaled by (1/8*A_mixed). We drop this uniform scale
* factor because the solution of the linear equations doesn't rely on it.
*
* The terms of the linear equations are xterm_dy with x in {a,b,c} and y in {a,b}. There are also const_dy
* terms that are the constant factors in the equations.
*/
/* find the vector from v along edge e */
vec_edge = Vec3r(-1 * e->GetVec());
ve2 = vec_edge.squareNorm();
vdotN = vec_edge * N;
/* section 5.2 - There is a typo in the computation of kappa. The edges should be x_j-x_i. */
kappa = 2.0 * vdotN / ve2;
/* section 5.2 */
/* I don't like performing a minimization where some of the weights can be negative (as can be the case
* if f1 or f2 are obtuse). To ensure all-positive weights, we check for obtuseness. */
weight = 0.0;
if (!triangle_obtuse(v, f1)) {
weight += ve2 * cotan(f1->GetNextOEdge(e->twin())->GetbVertex(), e->GetaVertex(), e->GetbVertex()) / 8.0;
}
else {
if (angle_obtuse(v, f1)) {
weight += ve2 * f1->getArea() / 4.0;
}
else {
weight += ve2 * f1->getArea() / 8.0;
}
}
if (!triangle_obtuse(v, f2)) {
weight += ve2 * cotan (f2->GetNextOEdge(e)->GetbVertex(), e->GetaVertex(), e->GetbVertex()) / 8.0;
}
else {
if (angle_obtuse(v, f2)) {
weight += ve2 * f1->getArea() / 4.0;
}
else {
weight += ve2 * f1->getArea() / 8.0;
}
}
/* projection of edge perpendicular to N (section 5.3) */
d[0] = vec_edge[0] - vdotN * N[0];
d[1] = vec_edge[1] - vdotN * N[1];
d[2] = vec_edge[2] - vdotN * N[2];
d.normalize();
/* not explicit in the paper, but necessary. Move d to 2D basis. */
d1 = d * basis1;
d2 = d * basis2;
/* store off the curvature, direction of edge, and weights for later use */
weights[edge_count] = weight;
kappas[edge_count] = kappa;
d1s[edge_count] = d1;
d2s[edge_count] = d2;
edge_count++;
/* Finally, update the linear equations */
aterm_da += weight * d1 * d1 * d1 * d1;
bterm_da += weight * d1 * d1 * 2 * d1 * d2;
cterm_da += weight * d1 * d1 * d2 * d2;
const_da += weight * d1 * d1 * (-kappa);
aterm_db += weight * d1 * d2 * d1 * d1;
bterm_db += weight * d1 * d2 * 2 * d1 * d2;
cterm_db += weight * d1 * d2 * d2 * d2;
const_db += weight * d1 * d2 * (-kappa);
}
/* now use the identity (Section 5.3) a + c = |Kh| = 2 * kappa_h */
aterm_da -= cterm_da;
const_da += cterm_da * normKh;
aterm_db -= cterm_db;
const_db += cterm_db * normKh;
/* check for solvability of the linear system */
if (((aterm_da * bterm_db - aterm_db * bterm_da) != 0.0) && ((const_da != 0.0) || (const_db != 0.0))) {
linsolve(aterm_da, bterm_da, -const_da, aterm_db, bterm_db, -const_db, &a, &b);
c = normKh - a;
eigenvector(a, b, c, eig);
}
else {
/* region of v is planar */
eig[0] = 1.0;
eig[1] = 0.0;
}
/* Although the eigenvectors of B are good estimates of the principal directions, it seems that which one is
* attached to which curvature direction is a bit arbitrary. This may be a bug in my implementation, or just
* a side-effect of the inaccuracy of B due to the discrete nature of the sampling.
*
* To overcome this behavior, we'll evaluate which assignment best matches the given eigenvectors by comparing
* the curvature estimates computed above and the curvatures calculated from the discrete differential operators.
*/
gts_vertex_principal_curvatures(0.5 * normKh, Kg, &K1, &K2);
err_e1 = err_e2 = 0.0;
/* loop through the values previously saved */
for (e = 0; e < edge_count; e++) {
real weight, kappa, d1, d2;
real temp1, temp2;
real delta;
weight = weights[e];
kappa = kappas[e];
d1 = d1s[e];
d2 = d2s[e];
temp1 = fabs (eig[0] * d1 + eig[1] * d2);
temp1 = temp1 * temp1;
temp2 = fabs (eig[1] * d1 - eig[0] * d2);
temp2 = temp2 * temp2;
/* err_e1 is for K1 associated with e1 */
delta = K1 * temp1 + K2 * temp2 - kappa;
err_e1 += weight * delta * delta;
/* err_e2 is for K1 associated with e2 */
delta = K2 * temp1 + K1 * temp2 - kappa;
err_e2 += weight * delta * delta;
}
free (weights);
free (kappas);
free (d1s);
free (d2s);
/* rotate eig by a right angle if that would decrease the error */
if (err_e2 < err_e1) {
real temp = eig[0];
eig[0] = eig[1];
eig[1] = -temp;
}
e1[0] = eig[0] * basis1[0] + eig[1] * basis2[0];
e1[1] = eig[0] * basis1[1] + eig[1] * basis2[1];
e1[2] = eig[0] * basis1[2] + eig[1] * basis2[2];
e1.normalize();
/* make N,e1,e2 a right handed coordinate sytem */
e2 = N ^ e1;
e2.normalize();
}
