bool Cylinder::intersects(const Vector3D& v0,const Vector3D& v1,Vector3D* intersection,Vector3D* normal) const
{
if(!intersection) return false;
// mindist between line directed by tDir and line(v0 v1).
Vector3D u = v1 - v0; u.normalize();
if(tDirection == u || tDirection == -u) // colinear
{
float dist = dotProduct(tDirection,v0 - getTransformedPosition());
Vector3D ext = v0 - (tDirection*dist + getTransformedPosition());
float r = ext.getNorm(); ext = ext / r;
if(r == radius) //intersection
{
*intersection = getTransformedPosition() + ext * radius;
if(normal)
*normal = computeNormal(*intersection);
return true;
}
else if(r < radius)
{
*intersection = getTransformedPosition() + tDirection * length*0.5f + ext * r;
if(normal)
*normal = computeNormal(*intersection);
return true;
}
return false;
}
else
{
Vector3D pp = getTransformedPosition() - v0, uv = u;
uv.crossProduct(tDirection);
float dist = std::abs(dotProduct(pp,uv))/uv.getNorm();
float d = dotProduct(tDirection,v0 - getTransformedPosition());
Vector3D ext = v0 - (tDirection*d + getTransformedPosition());
float r = ext.getNorm();
float ah = std::cos(std::asin(dist/r))*r;
Vector3D h = v0 + u*ah;
if(contains(h)) // intersection
{
float offset = 3.1415926535897932384626433832795f*0.5f*dist/radius;
*intersection = h - offset * u;
if(normal)
*normal = computeNormal(*intersection);
return true;
}
return false;
}
}
bool Plane::intersects(const Vector3D& v0,const Vector3D& v1,Vector3D* intersection,Vector3D* normal) const
{
float dist0 = dotProduct(tNormal,v0 - getTransformedPosition());
float dist1 = dotProduct(tNormal,v1 - getTransformedPosition());
if ((dist0 <= 0.0f) == (dist1 <= 0.0f)) // both points are on the same side
return false;
if (intersection != NULL)
{
if (dist0 <= 0.0f)
dist0 = -dist0;
else
dist1 = -dist1;
if (normal != NULL)
*normal = tNormal;
float ti = dist0 / (dist0 + dist1);
Vector3D vDir = v1 - v0;
float norm = vDir.getNorm();
norm *= ti;
ti = norm < APPROXIMATION_VALUE ? 0.0f : ti * (norm - APPROXIMATION_VALUE) / norm;
vDir *= ti;
*intersection = v0 + vDir;
}
return true;
}
void Cylinder::moveAtBorder(Vector3D& v,bool inside) const
{
float approx = inside ? -APPROXIMATION_VALUE : APPROXIMATION_VALUE;
float dist = dotProduct(tDirection,v - getTransformedPosition());
Vector3D ext = v - (tDirection*dist + getTransformedPosition());
float r = ext.getNorm(); ext = ext / r;
if(dist > length*0.5f)
{
v -= tDirection * (dist - length*0.5f - approx);
if(r > radius)
v -= ext*(r-radius-approx);
return;
}
else if(dist < -length * 0.5f)
{
v += tDirection * (length*0.5f - dist - approx);
if(r > radius)
v -= ext*(r-radius-approx);
return;
}
if(r > radius)
v -= ext*(r-radius-approx);
else
v += ext*(radius-r+approx);
}
bool Cylinder::contains(const Vector3D& v) const
{
float dist = dotProduct(tDirection,v - getTransformedPosition());
Vector3D ext = v - (tDirection*dist + getTransformedPosition());
float r = ext.getNorm();
return dist <= length*0.5f && dist >= -length*0.5f && r <= radius;
}
Vector3D Cylinder::computeNormal(const Vector3D& point) const
{
float dist = dotProduct(tDirection,point - getTransformedPosition());
if(dist >= length*0.5f) return tDirection;
if(dist <= -length*0.5f) return -tDirection;
Vector3D ext = point - (tDirection*dist + getTransformedPosition());
float r = ext.getNorm(); ext = ext / r;
return ext;
}
void Sphere::moveAtBorder(Vector3D& v,bool inside) const
{
Vector3D vDir = v - getTransformedPosition();
float norm = vDir.getNorm();
if (inside)
vDir *= (radius + APPROXIMATION_VALUE) / norm;
else
vDir *= (radius - APPROXIMATION_VALUE) / norm;
v = getTransformedPosition() + vDir;
}
bool Sphere::intersects(const Vector3D& v0,const Vector3D& v1,Vector3D* intersection,Vector3D* normal) const
{
float r2 = radius * radius;
float dist0 = getSqrDist(getTransformedPosition(),v0);
float dist1 = getSqrDist(getTransformedPosition(),v1);
if ((dist0 <= r2) == (dist1 <= r2))
return false;
if (intersection != NULL)
{
Vector3D vDir = v1 - v0;
float norm = vDir.getNorm();
float d = dotProduct(vDir,getTransformedPosition() - v0) / norm;
float a = std::sqrt(r2 - dist0 + d * d);
float ti;
if (dist0 <= r2)
ti = d - a;
else
ti = d + a;
ti /= norm;
if (ti < 0.0f) ti = 0.0f;
if (ti > 1.0f) ti = 1.0f;
norm *= ti;
ti = norm < APPROXIMATION_VALUE ? 0.0f : ti * (norm - APPROXIMATION_VALUE) / norm;
vDir *= ti;
*intersection = v0 + vDir;
if (normal != NULL)
{
if (dist0 <= r2)
*normal = getTransformedPosition() - *intersection;
else
*normal = *intersection - getTransformedPosition();
normal->normalize();
}
}
return true;
}
float Group::addParticles(const Vector3D& start,const Vector3D& end,const Vector3D& velocity,float step,float offset)
{
if ((step <= 0.0f)||(offset < 0.0f))
return 0.0f;
Vector3D displacement = end - start;
float totalDist = displacement.getNorm();
while(offset < totalDist)
{
Vector3D position = start;
position += displacement * (offset / totalDist);
addParticles(1,position,velocity,NULL,NULL);
offset += step;
}
return offset - totalDist;
}
if ((tEnter <= 0.0f)&&(tExit >= 1.0f))
return false;
if (intersection != NULL)
{
if (tEnter <= 0.0f)
{
tEnter = tExit;
axis = (axis & 0xF0) >> 4;
}
else
axis &= 0x0F;
Vector3D vDir = v1 - v0;
float norm = vDir.getNorm() * tEnter;
tEnter = norm < APPROXIMATION_VALUE ? 0.0f : tEnter * (norm - APPROXIMATION_VALUE) / norm;
vDir *= tEnter;
*intersection = v0 + vDir;
if (normal != NULL)
{
switch(axis)
{
case 0 :
*normal = Vector3D(-1.0f,0.0f,0.0f);
break;
case 1 :
*normal = Vector3D(0.0f,-1.0f,0.0f);
break;
