QList<RVector> RSpline::getPointsWithDistanceToEnd(double distance, int from) const {
QList<RVector> ret;
if (splineProxy!=NULL) {
double t;
if (from&RS::FromStart) {
t = splineProxy->getTAtDistance(*this, distance);
ret << getPointAt(t);
}
if (from&RS::FromEnd) {
t = splineProxy->getTAtDistance(*this, getLength() - distance);
ret << getPointAt(t);
}
}
else {
// no spline proxy (not precise, but better than nothing in some cases):
double length = getLength();
if (length<=RS::PointTolerance) {
return ret;
}
if (from&RS::FromStart) {
RVector p = getPointAt(getTMin() + (distance/length*getTDelta()));
ret.append(p);
}
if (from&RS::FromEnd) {
RVector p = getPointAt(getTMin() + ((length-distance)/length*getTDelta()));
ret.append(p);
}
}
return ret;
}
QuantisedFunctionPtr LineicModel::getUToArcLengthMapping() const
{
real_t totlength = getLength();
real_t fk = getFirstKnot();
real_t lk = getLastKnot();
uint_t stride = getStride();
real_t deltau = (lk - fk)/stride;
Vector3 p1 = getPointAt(fk);
Vector3 p2;
real_t length = 0;
real_t n = 0;
Point2ArrayPtr points(new Point2Array(stride+1));
points->setAt(0,Vector2(fk,0));
real_t u = fk + deltau;
for(uint_t i = 1 ; i <= stride; ++i, u += deltau){
p2 = getPointAt(u);
n = norm(p2 - p1);
length += n;
p1 = p2;
points->setAt(i,Vector2(u,length/totlength));
}
points->setAt(stride,Vector2(lk,1.0));
return QuantisedFunctionPtr(new QuantisedFunction(points,5*stride));
}
/**
* \return List of RLines describing this spline.
*/
QList<QSharedPointer<RShape> > RSpline::getExploded(int segments) const {
if (!exploded.isEmpty() && segments==-1) {
return exploded;
}
//qDebug() << "RSpline::getExploded: segments: " << segments;
//RDebug::printBacktrace("getExploded: ");
//##boundingBox = RBox();
updateInternal();
exploded.clear();
if (!isValid()) {
//qWarning() << "RSpline::getExploded: invalid spline";
return exploded;
}
if (segments==-1) {
segments = 8;
}
double tMin = getTMin();
double tMax = getTMax();
double step = getTDelta() / (controlPoints.size() * segments);
RVector p1;
RVector prev = RVector::invalid;
for (double t = tMin; t<tMax+(step/2.0); t+=step) {
double tc = qMin(t, tMax);
p1 = getPointAt(tc);
if (RMath::isNaN(p1.x) || RMath::isNaN(p1.y)) {
continue;
}
if (prev.isValid()) {
RLine* line = new RLine(prev, p1);
exploded.append(QSharedPointer<RShape>(line));
}
prev = p1;
//##boundingBox.growToInclude(p1);
}
p1 = getEndPoint();
if (!RMath::isNaN(p1.x) && !RMath::isNaN(p1.y)) {
if (prev.isValid()) {
RLine* line = new RLine(prev, p1);
// prevent zero length line at the end:
if (line->getLength()>1.0e-4) {
exploded.append(QSharedPointer<RShape>(line));
}
}
}
return exploded;
}
bool intersect(const Ray& _ray, const Plane& _plane, Intersection* _intersection)
{
float equation = bx::vec3Dot(_ray.m_pos, _plane.m_normal) + _plane.m_dist;
if (0.0f > equation)
{
return false;
}
float ndotd = bx::vec3Dot(_ray.m_dir, _plane.m_normal);
if (0.0f < ndotd)
{
return false;
}
if (NULL != _intersection)
{
bx::vec3Move(_intersection->m_normal, _plane.m_normal);
float tt = -equation/ndotd;
_intersection->m_dist = tt;
getPointAt(_intersection->m_pos, _ray, tt);
}
return true;
}
Point LineSegment::intersection(LineSegment line)
{
float c1, c2;
float intersection_X = -1, intersection_Y= -1;
c1 = p1.y - slope * p1.x; // which is same as y2 - slope * x2
c2 = line.p2.y - line.slope * line.p2.x; // which is same as y2 - slope * x2
if( (slope - line.slope) == 0)
{
//std::cout << "No Intersection between the lines" << endl;
}
else if (p1.x == p2.x)
{
// Line1 is vertical
return Point(p1.x, line.getPointAt(p1.x));
}
else if (line.p1.x == line.p2.x)
{
// Line2 is vertical
return Point(line.p1.x, getPointAt(line.p1.x));
}
else
{
intersection_X = (c2 - c1) / (slope - line.slope);
intersection_Y = slope * intersection_X + c1;
}
return Point(intersection_X, intersection_Y);
}
QList<RVector> RSpline::getDiscontinuities() const {
updateInternal();
QList<RVector> ret;
#ifndef R_NO_OPENNURBS
if (isValid()) {
for (int c=0; c<=11; c++) {
double t0=getTMin();
double t1=getTMax();
bool found;
do {
double t;
found = curve.GetNextDiscontinuity((ON::continuity)c, t0, t1, &t);
if (found) {
ret.append(getPointAt(t));
t0=t;
}
} while(found);
}
}
#endif
return ret;
}
void svg::Polygon::print(std::ostream& os)
{
os <<"polygon ";
for(unsigned int i = 0; i < getNumPoints(); ++i) {
if (i > 0)
os << ", ";
os << getPointAt(i);
}
}
bool RSpline::isOnShape(const RVector& point, bool limited, double tolerance) const {
if (hasProxy()) {
double t = getTAtPoint(point);
RVector p = getPointAt(t);
return point.getDistanceTo(p) < tolerance;
}
else {
return RShape::isOnShape(point, limited, tolerance);
}
}
QList<RVector> RSpline::getPointsWithDistanceToEnd(double distance, RS::From from) const {
QList<RVector> ret;
double length = getLength();
if (length<=RS::PointTolerance) {
return ret;
}
if (from==RS::FromStart || from==RS::FromAny) {
RVector p = getPointAt(getTMin() + (distance/length*getTDelta()));
ret.append(p);
}
if (from==RS::FromEnd || from==RS::FromAny) {
RVector p = getPointAt(getTMin() + ((length-distance)/length*getTDelta()));
ret.append(p);
}
return ret;
}
real_t
LineicModel::getLength(real_t begin, real_t end) const
{
real_t fk = getFirstKnot();
real_t lk = getLastKnot();
if (begin < getFirstKnot()) begin = fk;
if (end > getLastKnot()) end = lk;
real_t deltau = (lk - fk)/getStride();
// We use the same u sequence to compute the length
// For this, we compute the closer smaller u value from begin and end
real_t beginI = int((begin-fk)/deltau) * deltau + fk;
real_t endI = int((end-fk)/deltau) * deltau + fk;
Vector3 p1 = getPointAt(beginI);
Vector3 p2;
real_t length = 0;
// Eventually we do some adjustement according to the real begin and end values
// here and just after the loop
if (begin-beginI > GEOM_EPSILON){
p2 = getPointAt(begin);
length -= norm(p2 - p1);
}
for(real_t u = beginI + fk + deltau ; u <= endI ; u += deltau){
p2 = getPointAt(u);
length += norm(p2 - p1);
p1 = p2;
}
if (end-endI > GEOM_EPSILON){
p2 = getPointAt(end);
length += norm(p2 - p1);
}
return length;
}
Vector3
LineicModel::findClosest(const Vector3& p, real_t* ui) const{
real_t u0 = getFirstKnot();
real_t u1 = getLastKnot();
real_t deltau = (u1 - u0)/getStride();
Vector3 p1 = getPointAt(u0);
Vector3 res = p1;
real_t dist = normSquared(p-res);
Vector3 p2, pt;
real_t lu;
for(real_t u = u0 + deltau ; u <= u1 ; u += deltau){
p2 = getPointAt(u);
pt = p;
real_t d = __closestPointToSegment(pt,p1,p2,&lu);
if(d < dist){
dist = d;
res = pt;
if (ui != NULL) *ui = u + deltau * (lu -1);
}
p1 = p2;
}
return res;
}
Point LineSegment::midpoint()
{
// Handle the case where the line is vertical
if (p1.x == p2.x)
{
float ydiff = p2.y-p1.y;
float y = p1.y + (ydiff/2);
return Point(p1.x, y);
}
float diff = p2.x - p1.x;
float midX = ((float) p1.x) + (diff / 2);
int midY = getPointAt(midX);
return Point(midX, midY);
}
QuantisedFunctionPtr LineicModel::getArcLengthToUMapping() const
{
real_t totlength = getLength();
real_t fk = getFirstKnot();
real_t lk = getLastKnot();
uint_t stride = getStride();
real_t deltau = (lk - fk)/stride;
Vector3 p1 = getPointAt(fk);
Vector3 p2;
real_t length = 0;
real_t n = 0;
Point2ArrayPtr points(new Point2Array(stride+1));
points->setAt(0,Vector2(0,fk));
real_t u = fk + deltau;
uint_t j = 1;
for(uint_t i = 1 ; i <= stride; ++i, u += deltau){
p2 = getPointAt(u);
n = norm(p2 - p1);
if (n > 0){
length += n;
p1 = p2;
points->setAt(j,Vector2(length/totlength,u));
++j;
}
}
points->setAt(j-1,Vector2(1.0,lk));
if (j != stride+1){
points = Point2ArrayPtr(new Point2Array(points->begin(),points->begin()+j));
}
return QuantisedFunctionPtr(new QuantisedFunction(points,5*stride));
}
bool intersect(const Ray& _ray, const Tris& _triangle, Intersection* _intersection)
{
float edge10[3];
bx::vec3Sub(edge10, _triangle.m_v1, _triangle.m_v0);
float edge02[3];
bx::vec3Sub(edge02, _triangle.m_v0, _triangle.m_v2);
float normal[3];
bx::vec3Cross(normal, edge02, edge10);
float vo[3];
bx::vec3Sub(vo, _triangle.m_v0, _ray.m_pos);
float dxo[3];
bx::vec3Cross(dxo, _ray.m_dir, vo);
const float det = bx::vec3Dot(normal, _ray.m_dir);
if (det > 0.0f)
{
return false;
}
const float invDet = 1.0f/det;
const float bz = bx::vec3Dot(dxo, edge02) * invDet;
const float by = bx::vec3Dot(dxo, edge10) * invDet;
const float bx = 1.0f - by - bz;
if (bx < 0.0f || by < 0.0f || bz < 0.0f)
{
return false;
}
if (NULL != _intersection)
{
bx::vec3Norm(_intersection->m_normal, normal);
const float tt = bx::vec3Dot(normal, vo) * invDet;
_intersection->m_dist = tt;
getPointAt(_intersection->m_pos, _ray, tt);
}
return true;
}
bool intersect(const Ray& _ray, const Sphere& _sphere, Intersection* _intersection)
{
float rs[3];
bx::vec3Sub(rs, _ray.m_pos, _sphere.m_center);
const float bb = bx::vec3Dot(rs, _ray.m_dir);
if (0.0f < bb)
{
return false;
}
const float aa = bx::vec3Dot(_ray.m_dir, _ray.m_dir);
const float cc = bx::vec3Dot(rs, rs) - bx::fsq(_sphere.m_radius);
const float discriminant = bb*bb - aa*cc;
if (0.0f >= discriminant)
{
return false;
}
const float sqrtDiscriminant = bx::fsqrt(discriminant);
const float invA = 1.0f / aa;
const float tt = -(bb + sqrtDiscriminant)*invA;
if (0.0f >= tt)
{
return false;
}
if (NULL != _intersection)
{
_intersection->m_dist = tt;
float point[3];
getPointAt(point, _ray, tt);
bx::vec3Move(_intersection->m_pos, point);
float tmp[3];
bx::vec3Sub(tmp, point, _sphere.m_center);
bx::vec3Norm(_intersection->m_normal, tmp);
}
return true;
}
bool intersect(const Ray& _ray, const Aabb& _aabb, Intersection* _intersection)
{
float invDir[3];
bx::vec3Rcp(invDir, _ray.m_dir);
float tmp[3];
float t0[3];
bx::vec3Sub(tmp, _aabb.m_min, _ray.m_pos);
bx::vec3Mul(t0, tmp, invDir);
float t1[3];
bx::vec3Sub(tmp, _aabb.m_max, _ray.m_pos);
bx::vec3Mul(t1, tmp, invDir);
float min[3];
bx::vec3Min(min, t0, t1);
float max[3];
bx::vec3Max(max, t0, t1);
const float tmin = bx::fmax3(min[0], min[1], min[2]);
const float tmax = bx::fmin3(max[0], max[1], max[2]);
if (tmax < 0.0f
|| tmin > tmax)
{
return false;
}
if (NULL != _intersection)
{
_intersection->m_normal[0] = float( (min[0] == tmin) - (max[0] == tmin) );
_intersection->m_normal[1] = float( (min[1] == tmin) - (max[1] == tmin) );
_intersection->m_normal[2] = float( (min[2] == tmin) - (max[2] == tmin) );
_intersection->m_dist = tmin;
getPointAt(_intersection->m_pos, _ray, tmin);
}
return true;
}
RVector RSpline::getMiddlePoint() const {
return getPointAt(getTMin() + (getTDelta()/2.0));
}
RVector RSpline::getPointAtDistance(double distance) const {
double t = getTAtDistance(distance);
return getPointAt(t);
}
RVector RSpline::getEndPoint() const {
return getPointAt(getTMax());
}
RVector RSpline::getStartPoint() const {
return getPointAt(getTMin());
}
bool intersect(const Ray& _ray, const Cylinder& _cylinder, bool _capsule, Intersection* _intersection)
{
float axis[3];
bx::vec3Sub(axis, _cylinder.m_end, _cylinder.m_pos);
float rc[3];
bx::vec3Sub(rc, _ray.m_pos, _cylinder.m_pos);
float normal[3];
bx::vec3Cross(normal, _ray.m_dir, axis);
const float len = bx::vec3Norm(normal, normal);
const float dist = bx::fabsolute(bx::vec3Dot(rc, normal) );
if (dist > _cylinder.m_radius)
{
return false;
}
float vo[3];
bx::vec3Cross(vo, rc, axis);
const float t0 = -bx::vec3Dot(vo, normal) / len;
bx::vec3Cross(vo, normal, axis);
bx::vec3Norm(vo, vo);
const float rsq = bx::fsq(_cylinder.m_radius);
const float ddoto = bx::vec3Dot(_ray.m_dir, vo);
const float ss = t0 - bx::fabsolute(bx::fsqrt(rsq - bx::fsq(dist) ) / ddoto);
float point[3];
getPointAt(point, _ray, ss);
const float axisLen = bx::vec3Norm(axis, axis);
const float pdota = bx::vec3Dot(_cylinder.m_pos, axis);
const float height = bx::vec3Dot(point, axis) - pdota;
if (height > 0.0f
&& height < axisLen)
{
if (NULL != _intersection)
{
const float t1 = height / axisLen;
float pointOnAxis[3];
bx::vec3Lerp(pointOnAxis, _cylinder.m_pos, _cylinder.m_end, t1);
bx::vec3Move(_intersection->m_pos, point);
float tmp[3];
bx::vec3Sub(tmp, point, pointOnAxis);
bx::vec3Norm(_intersection->m_normal, tmp);
_intersection->m_dist = ss;
}
return true;
}
if (_capsule)
{
const float rdota = bx::vec3Dot(_ray.m_pos, axis);
const float pp = rdota - pdota;
const float t1 = pp / axisLen;
float pointOnAxis[3];
bx::vec3Lerp(pointOnAxis, _cylinder.m_pos, _cylinder.m_end, t1);
float axisToRay[3];
bx::vec3Sub(axisToRay, _ray.m_pos, pointOnAxis);
if (_cylinder.m_radius < bx::vec3Length(axisToRay)
&& 0.0f > ss)
{
return false;
}
Sphere sphere;
sphere.m_radius = _cylinder.m_radius;
bx::vec3Move(sphere.m_center, 0.0f >= height
? _cylinder.m_pos
: _cylinder.m_end
);
return intersect(_ray, sphere, _intersection);
}
Plane plane;
float pos[3];
if (0.0f >= height)
{
bx::vec3Neg(plane.m_normal, axis);
bx::vec3Move(pos, _cylinder.m_pos);
}
else
{
bx::vec3Move(plane.m_normal, axis);
bx::vec3Move(pos, _cylinder.m_end);
}
plane.m_dist = -bx::vec3Dot(pos, plane.m_normal);
Intersection tmpIntersection;
_intersection = NULL != _intersection ? _intersection : &tmpIntersection;
if (intersect(_ray, plane, _intersection) )
{
float tmp[3];
bx::vec3Sub(tmp, pos, _intersection->m_pos);
return bx::vec3Dot(tmp, tmp) <= rsq;
}
return false;
}
RVector REllipse::getMiddlePoint() const {
double a;
a = getStartParam() + getSweep()/2.0;
return getPointAt(a);
}
