bool Box::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const {
// handle the trivial case where the box contains the origin
if (contains(origin)) {
distance = 0.0f;
return true;
}
// check each axis
float axisDistance;
if ((findIntersection(origin.x, direction.x, minimum.x, maximum.x, axisDistance) && axisDistance >= 0 &&
isWithin(origin.y + axisDistance*direction.y, minimum.y, maximum.y) &&
isWithin(origin.z + axisDistance*direction.z, minimum.z, maximum.z))) {
distance = axisDistance;
return true;
}
if ((findIntersection(origin.y, direction.y, minimum.y, maximum.y, axisDistance) && axisDistance >= 0 &&
isWithin(origin.x + axisDistance*direction.x, minimum.x, maximum.x) &&
isWithin(origin.z + axisDistance*direction.z, minimum.z, maximum.z))) {
distance = axisDistance;
return true;
}
if ((findIntersection(origin.z, direction.z, minimum.z, maximum.z, axisDistance) && axisDistance >= 0 &&
isWithin(origin.y + axisDistance*direction.y, minimum.y, maximum.y) &&
isWithin(origin.x + axisDistance*direction.x, minimum.x, maximum.x))) {
distance = axisDistance;
return true;
}
return false;
}
bool AABox::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance, BoxFace& face) const {
// handle the trivial case where the box contains the origin
if (contains(origin)) {
distance = 0;
return true;
}
// check each axis
float axisDistance;
if ((findIntersection(origin.x, direction.x, _corner.x, _scale, axisDistance) && axisDistance >= 0 &&
isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale) &&
isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale))) {
distance = axisDistance;
face = direction.x > 0 ? MIN_X_FACE : MAX_X_FACE;
return true;
}
if ((findIntersection(origin.y, direction.y, _corner.y, _scale, axisDistance) && axisDistance >= 0 &&
isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale) &&
isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale))) {
distance = axisDistance;
face = direction.y > 0 ? MIN_Y_FACE : MAX_Y_FACE;
return true;
}
if ((findIntersection(origin.z, direction.z, _corner.z, _scale, axisDistance) && axisDistance >= 0 &&
isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale) &&
isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale))) {
distance = axisDistance;
face = direction.z > 0 ? MIN_Z_FACE : MAX_Z_FACE;
return true;
}
return false;
}
double Robby::getDistance() {
float xPrime = cos(getRotation()*0.0174533)*hostWindow.getSize().x*2;
float yPrime = sin(getRotation()*0.0174533)*hostWindow.getSize().x*2;
sf::Vector2f pPoint(xPrime,yPrime);
sf::Vector2f oPoint = getPosition();
sf::Vector2f tlPoint(0,0);
sf::Vector2f trPoint(hostWindow.getSize().x,0);
sf::Vector2f blPoint(0,hostWindow.getSize().y);
sf::Vector2f brPoint(hostWindow.getSize().x,hostWindow.getSize().y);
sf::Vector2f topLine = findIntersection(oPoint,pPoint,tlPoint,trPoint);
sf::Vector2f botLine = findIntersection(oPoint,pPoint,blPoint,brPoint);
sf::Vector2f leftLine = findIntersection(oPoint,pPoint,tlPoint,blPoint);
sf::Vector2f rightLine = findIntersection(oPoint,pPoint,trPoint,brPoint);
sf::Vector2f intercept;
if (topLine.x < -9) {
if (botLine.x < -9) {
if (leftLine.x < -9) {
intercept = rightLine;
} else intercept = leftLine;
} else intercept = botLine;
} else intercept = topLine;
double distance = sqrt(pow(intercept.x-getPosition().x,2)+pow(intercept.y-getPosition().y,2));
return distance;
}
bool AACube::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance, BoxFace& face) const {
// handle the trivial case where the box contains the origin
if (contains(origin)) {
// We still want to calculate the distance from the origin to the inside out plane
float axisDistance;
if ((findInsideOutIntersection(origin.x, direction.x, _corner.x, _scale, axisDistance) && axisDistance >= 0 &&
isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale) &&
isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale))) {
distance = axisDistance;
face = direction.x > 0 ? MAX_X_FACE : MIN_X_FACE;
return true;
}
if ((findInsideOutIntersection(origin.y, direction.y, _corner.y, _scale, axisDistance) && axisDistance >= 0 &&
isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale) &&
isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale))) {
distance = axisDistance;
face = direction.y > 0 ? MAX_Y_FACE : MIN_Y_FACE;
return true;
}
if ((findInsideOutIntersection(origin.z, direction.z, _corner.z, _scale, axisDistance) && axisDistance >= 0 &&
isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale) &&
isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale))) {
distance = axisDistance;
face = direction.z > 0 ? MAX_Z_FACE : MIN_Z_FACE;
return true;
}
// This case is unexpected, but mimics the previous behavior for inside out intersections
distance = 0;
return true;
}
// check each axis
float axisDistance;
if ((findIntersection(origin.x, direction.x, _corner.x, _scale, axisDistance) && axisDistance >= 0 &&
isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale) &&
isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale))) {
distance = axisDistance;
face = direction.x > 0 ? MIN_X_FACE : MAX_X_FACE;
return true;
}
if ((findIntersection(origin.y, direction.y, _corner.y, _scale, axisDistance) && axisDistance >= 0 &&
isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale) &&
isWithin(origin.z + axisDistance*direction.z, _corner.z, _scale))) {
distance = axisDistance;
face = direction.y > 0 ? MIN_Y_FACE : MAX_Y_FACE;
return true;
}
if ((findIntersection(origin.z, direction.z, _corner.z, _scale, axisDistance) && axisDistance >= 0 &&
isWithin(origin.y + axisDistance*direction.y, _corner.y, _scale) &&
isWithin(origin.x + axisDistance*direction.x, _corner.x, _scale))) {
distance = axisDistance;
face = direction.z > 0 ? MIN_Z_FACE : MAX_Z_FACE;
return true;
}
return false;
}
template <typename PointNT> void
pcl::GridProjection<PointNT>::findIntersection (int level,
const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
const Eigen::Vector4f &start_pt,
std::vector <int> &pt_union_indices,
Eigen::Vector4f &intersection)
{
assert (end_pts.size () == 2);
assert (vect_at_end_pts.size () == 2);
Eigen::Vector3f vec;
getVectorAtPoint (start_pt, pt_union_indices, vec);
double d1 = getD1AtPoint (start_pt, vec, pt_union_indices);
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > new_end_pts (2);
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > new_vect_at_end_pts (2);
if ((fabs (d1) < 10e-3) || (level == max_binary_search_level_))
{
intersection = start_pt;
return;
}
else
{
vec.normalize ();
if (vec.dot (vect_at_end_pts[0]) < 0)
{
Eigen::Vector4f new_start_pt = end_pts[0] + (start_pt - end_pts[0]) * 0.5;
new_end_pts[0] = end_pts[0];
new_end_pts[1] = start_pt;
new_vect_at_end_pts[0] = vect_at_end_pts[0];
new_vect_at_end_pts[1] = vec;
findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
return;
}
if (vec.dot (vect_at_end_pts[1]) < 0)
{
Eigen::Vector4f new_start_pt = start_pt + (end_pts[1] - start_pt) * 0.5;
new_end_pts[0] = start_pt;
new_end_pts[1] = end_pts[1];
new_vect_at_end_pts[0] = vec;
new_vect_at_end_pts[1] = vect_at_end_pts[1];
findIntersection (level + 1, new_end_pts, new_vect_at_end_pts, new_start_pt, pt_union_indices, intersection);
return;
}
intersection = start_pt;
return;
}
}
void AutoNavigation::Private::adjustZoom( const GeoDataCoordinates ¤tPosition, qreal speed )
{
qreal currentX = 0;
qreal currentY = 0;
if( !m_viewport->screenCoordinates(currentPosition, currentX, currentY ) ) {
return;
}
const GeoDataCoordinates destination = findIntersection( currentX, currentY );
qreal greatCircleDistance = distanceSphere( currentPosition, destination );
qreal radius = m_model->planetRadius();
qreal distance = greatCircleDistance * radius;
if( speed != 0 ) {
// time (in seconds) remaining to reach the border of the map
qreal remainingTime = distance / speed;
// tolerance time limits (in seconds) before auto zooming
qreal thresholdLow = 15;
qreal thresholdHigh = 120;
m_selfInteraction = true;
if ( remainingTime < thresholdLow ) {
emit m_parent->zoomOut( Instant );
}
else if ( remainingTime > thresholdHigh ) {
emit m_parent->zoomIn( Instant );
}
m_selfInteraction = false;
}
}
template <typename PointNT> void
pcl::GridProjection<PointNT>::getProjection (const Eigen::Vector4f &p,
std::vector <int> &pt_union_indices, Eigen::Vector4f &projection)
{
const double projection_distance = leaf_size_ * 3;
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pt (2);
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > end_pt_vect (2);
end_pt[0] = p;
getVectorAtPoint (end_pt[0], pt_union_indices, end_pt_vect[0]);
end_pt_vect[0].normalize();
double dSecond = getD2AtPoint (end_pt[0], end_pt_vect[0], pt_union_indices);
// Find another point which is projection_distance away from the p, do a
// binary search between these two points, to find the projected point on the
// surface
if (dSecond > 0)
end_pt[1] = end_pt[0] + Eigen::Vector4f (end_pt_vect[0][0] * projection_distance,
end_pt_vect[0][1] * projection_distance,
end_pt_vect[0][2] * projection_distance, 0);
else
end_pt[1] = end_pt[0] - Eigen::Vector4f (end_pt_vect[0][0] * projection_distance,
end_pt_vect[0][1] * projection_distance,
end_pt_vect[0][2] * projection_distance, 0);
getVectorAtPoint (end_pt[1], pt_union_indices, end_pt_vect[1]);
if (end_pt_vect[1].dot (end_pt_vect[0]) < 0)
{
Eigen::Vector4f mid_pt = end_pt[0] + (end_pt[1] - end_pt[0]) * 0.5;
findIntersection (0, end_pt, end_pt_vect, mid_pt, pt_union_indices, projection);
}
else
projection = p;
}
void PIDuser::calcSignals(std::vector<float> &state, float &gas, float &turn)
{
// Find point on reference curve.
m_refInd = findIntersection(state, m_startInd);
m_refSpeed = vRef[m_refInd];
m_refAngle = aRef[m_refInd];
if(m_refSpeed > state[2] + 0.5)
{
m_refSpeed = state[2] + 0.5;
}
// Calculate gas and turn signal.
//gas = calcGasSignal(state, m_refGas);
gas = calcGasSignalAlt(state, m_refSpeed);
//gas = m_refGas;
turn = calcTurnSignal(state, m_refInd);
EnterCriticalSection(&csPlotData);
if (UserSpeedPlotData.makePlot)
{
UserSpeedPlotData.Y[1][m_refInd] = state[2];
UserSpeedPlotData.newDataReady[1] = true;
}
LeaveCriticalSection(&csPlotData);
}
template <typename PointNT> bool
pcl::GridProjection<PointNT>::isIntersected (const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &end_pts,
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &vect_at_end_pts,
std::vector <int> &pt_union_indices)
{
assert (end_pts.size () == 2);
assert (vect_at_end_pts.size () == 2);
double length[2];
for (size_t i = 0; i < 2; ++i)
{
length[i] = vect_at_end_pts[i].norm ();
vect_at_end_pts[i].normalize ();
}
double dot_prod = vect_at_end_pts[0].dot (vect_at_end_pts[1]);
if (dot_prod < 0)
{
double ratio = length[0] / (length[0] + length[1]);
Eigen::Vector4f start_pt =
end_pts[0] + (end_pts[1] - end_pts[0]) * static_cast<float> (ratio);
Eigen::Vector4f intersection_pt = Eigen::Vector4f::Zero ();
findIntersection (0, end_pts, vect_at_end_pts, start_pt, pt_union_indices, intersection_pt);
Eigen::Vector3f vec;
getVectorAtPoint (intersection_pt, pt_union_indices, vec);
vec.normalize ();
double d2 = getD2AtPoint (intersection_pt, vec, pt_union_indices);
if (d2 < 0)
return (true);
}
return (false);
}
PointerEvent ParabolaPointer::buildPointerEvent(const PickedObject& target, const PickResultPointer& pickResult, const std::string& button, bool hover) {
QUuid pickedID;
glm::vec3 intersection, surfaceNormal, origin, velocity, acceleration;
auto parabolaPickResult = std::static_pointer_cast<ParabolaPickResult>(pickResult);
if (parabolaPickResult) {
intersection = parabolaPickResult->intersection;
surfaceNormal = parabolaPickResult->surfaceNormal;
const QVariantMap& parabola = parabolaPickResult->pickVariant;
origin = vec3FromVariant(parabola["origin"]);
velocity = vec3FromVariant(parabola["velocity"]);
acceleration = vec3FromVariant(parabola["acceleration"]);
pickedID = parabolaPickResult->objectID;
}
if (pickedID != target.objectID) {
intersection = findIntersection(target, origin, velocity, acceleration);
}
glm::vec2 pos2D = findPos2D(target, intersection);
// If we just started triggering and we haven't moved too much, don't update intersection and pos2D
TriggerState& state = hover ? _latestState : _states[button];
float sensorToWorldScale = DependencyManager::get<AvatarManager>()->getMyAvatar()->getSensorToWorldScale();
float deadspotSquared = TOUCH_PRESS_TO_MOVE_DEADSPOT_SQUARED * sensorToWorldScale * sensorToWorldScale;
bool withinDeadspot = usecTimestampNow() - state.triggerStartTime < POINTER_MOVE_DELAY && glm::distance2(pos2D, state.triggerPos2D) < deadspotSquared;
if ((state.triggering || state.wasTriggering) && !state.deadspotExpired && withinDeadspot) {
pos2D = state.triggerPos2D;
intersection = state.intersection;
surfaceNormal = state.surfaceNormal;
}
if (!withinDeadspot) {
state.deadspotExpired = true;
}
return PointerEvent(pos2D, intersection, surfaceNormal, velocity);
}
pair<DoorObject::corner_node_t*,size_t> DoorObject::corner_node_t::buildTree(vector<LineSegment> segments)
{
// this is harder than it seems... we need to know how things are connected. Will think about it.
// compute all intersections
vector<Coord> intersections;
//cerr << "building tree from " << segments.size() << " segments" << endl;
for(size_t i = 0; i < segments.size(); i++)
{
//cerr << "segments[" << i << "] = " << segments[i] << endl;
for(size_t j = i; j < segments.size(); j++)
{
Coord c = findIntersection(segments[i], segments[j]);
//cerr << " intersection between segments " << i << " and " << j << " : " << c << endl;
if(c != noCoord) intersections.push_back(c);
}
}
//cerr << "Found " << intersections.size() << " intersections:" << endl;
// convert array to corner_node_t
size_t N = intersections.size();
corner_node_t* nodes = new corner_node_t [N];
// copy in the data from intersections vector
for(size_t i = 0; i < N; i++)
{
nodes[i].point = intersections[i];
//cerr << " " << i << ": " << nodes[i].point << endl;
}
// for each segment, figure out the connections between intersections on that segment
for(size_t i = 0; i < segments.size(); i++) {
//cerr << segments[i] << endl;
// find all nodes on line, sort
vector<corner_node_t*> nodesOnLine;
for(size_t f = 0; f < N; f++) {
//cerr << " checking " << nodes[f].point;
if( segments[i].onLine(nodes[f].point) ) {
nodesOnLine.push_back( &nodes[f] );
//cerr << " yes";
}
//cerr << endl;
}
//cerr << " " << nodesOnLine.size() << " nodes on line " << endl;
sort(nodesOnLine.begin(), nodesOnLine.end(), DoorObject::corner_node_t::compareNodesByCoord); // sort them by coordinate position
// assert: items in nodesOnLine are sorted by position
// link all the items back-to front with connections
for(size_t n = 0; n < nodesOnLine.size()-1; n++)
{
nodesOnLine[n]->elt.push_back(nodesOnLine[n+1]);
nodesOnLine[n+1]->elt.push_back(nodesOnLine[n]);
}
}
pair<corner_node_t*,size_t> R (nodes, N);
return R;
}
// find intersection between two 2D segments
QVector<GLC_Point2d> glc::findIntersection(const GLC_Point2d& s1p1, const GLC_Point2d& s1p2, const GLC_Point2d& s2p1, const GLC_Point2d& s2p2)
{
const GLC_Vector2d D0= s1p2 - s1p1;
const GLC_Vector2d D1= s2p2 - s2p1;
// The QVector of result points
QVector<GLC_Point2d> result;
const GLC_Vector2d E(s2p1 - s1p1);
double kross= D0 ^ D1;
double sqrKross= kross * kross;
const double sqrLen0= D0.x() * D0.x() + D0.y() * D0.y();
const double sqrLen1= D1.x() * D1.x() + D1.y() * D1.y();
// Test if the line are nor parallel
if (sqrKross > (EPSILON * sqrLen0 * sqrLen1))
{
const double s= (E ^ D1) / kross;
if ((s < 0.0) || (s > 1.0))
{
// Intersection of lines is not a point on segment s1p1 + s * DO
return result; // Return empty QVector
}
const double t= (E ^ D0) / kross;
if ((t < 0.0) || (t > 1.0))
{
// Intersection of lines is not a point on segment s2p1 + t * D1
return result; // Return empty QVector
}
// Intersection of lines is a point on each segment
result << (s1p1 + (D0 * s));
return result; // Return a QVector of One Point
}
// Lines of the segments are parallel
const double sqrLenE= E.x() * E.x() + E.y() * E.y();
kross= E ^ D0;
sqrKross= kross * kross;
if (sqrKross > (EPSILON * sqrLen0 * sqrLenE))
{
// Lines of the segments are different
return result; // Return empty QVector
}
// Lines of the segments are the same. Need to test for overlap of segments.
const double s0= (D0 * E) / sqrLen0;
const double s1= (D0 * D1) / sqrLen0;
const double sMin= qMin(s0, s1);
const double sMax= qMax(s0, s1);
QVector<double> overlaps= findIntersection(0.0, 1.0, sMin, sMax);
const int iMax= overlaps.size();
for (int i= 0; i < iMax; ++i)
{
result.append(s1p1 + (D0 * overlaps[i]));
}
return result;
}
void printOut(int set_a[], int size_a, int set_b[], int size_b)
{
printf("the intersection of\n");
outputSets(set_a, size_a);
printf("and\n");
outputSets(set_b, size_b);
printf("is\n");
findIntersection(set_a, size_a, set_b, size_b);
}
HitRecord BezierCurve::intersect(const Ray &ray, const double dist, const double time) const {
HitRecord hit = HitRecord(dist);
//These are the endpoints of the line segment
double t = findIntersection(0.0, 1.0, ray);
if (t != 0.0){
hit.hit(t, this, matl, ray, n);
}
return hit;
}
double BezierCurve::findIntersection(const double t0, const double t1, const Ray &ray) const{
const double MINIMUM_LINE_DISTANCE = 0.03;
const double THRESHOLD = 0.003;
double tMid = (t0 + t1) / 2.0;
Vector o = ray.origin();
Vector pMid = calculateBezierPoint(tMid);
Vector d = ray.direction();
Vector gamma = pMid-o;
gamma.normalize();
double c = Dot(gamma, gamma);
double b = Dot(gamma, d);
double length = sqrt(c-b);
if(length < THRESHOLD) {
return tMid;
} else{
double lValue = 0;
double rValue = 0;
Vector p0 = calculateBezierPoint(t0);
Vector p1 = calculateBezierPoint(t1);
p0.normalize();
p1.normalize();
if ((p0-p1).length() < MINIMUM_LINE_DISTANCE){
return 0;
}
lValue = findIntersection(t0, tMid, ray);
rValue = findIntersection(tMid, t1, ray);
return (rValue > lValue) ? rValue : lValue;
}
}
bool AABox::expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& end, float expansion) const {
// handle the trivial cases where the expanded box contains the start or end
if (expandedContains(start, expansion) || expandedContains(end, expansion)) {
return true;
}
// check each axis
glm::vec3 expandedCorner = _corner - glm::vec3(expansion, expansion, expansion);
glm::vec3 expandedSize = glm::vec3(_scale, _scale, _scale) + glm::vec3(expansion, expansion, expansion) * 2.0f;
glm::vec3 direction = end - start;
float axisDistance;
return (findIntersection(start.x, direction.x, expandedCorner.x, expandedSize.x, axisDistance) &&
axisDistance >= 0.0f && axisDistance <= 1.0f &&
isWithin(start.y + axisDistance*direction.y, expandedCorner.y, expandedSize.y) &&
isWithin(start.z + axisDistance*direction.z, expandedCorner.z, expandedSize.z)) ||
(findIntersection(start.y, direction.y, expandedCorner.y, expandedSize.y, axisDistance) &&
axisDistance >= 0.0f && axisDistance <= 1.0f &&
isWithin(start.x + axisDistance*direction.x, expandedCorner.x, expandedSize.x) &&
isWithin(start.z + axisDistance*direction.z, expandedCorner.z, expandedSize.z)) ||
(findIntersection(start.z, direction.z, expandedCorner.z, expandedSize.z, axisDistance) &&
axisDistance >= 0.0f && axisDistance <= 1.0f &&
isWithin(start.y + axisDistance*direction.y, expandedCorner.y, expandedSize.y) &&
isWithin(start.x + axisDistance*direction.x, expandedCorner.x, expandedSize.x));
}
// return true if there is an intersection between 2 segments
bool glc::isIntersected(const GLC_Point2d& s1p1, const GLC_Point2d& s1p2, const GLC_Point2d& s2p1, const GLC_Point2d& s2p2)
{
const GLC_Vector2d D0= s1p2 - s1p1;
const GLC_Vector2d D1= s2p2 - s2p1;
const GLC_Vector2d E(s2p1 - s1p1);
double kross= D0 ^ D1;
double sqrKross= kross * kross;
const double sqrLen0= D0.x() * D0.x() + D0.y() * D0.y();
const double sqrLen1= D1.x() * D1.x() + D1.y() * D1.y();
// Test if the line are nor parallel
if (sqrKross > (EPSILON * sqrLen0 * sqrLen1))
{
const double s= (E ^ D1) / kross;
if ((s < 0.0) || (s > 1.0))
{
// Intersection of lines is not a point on segment s1p1 + s * DO
return false;
}
const double t= (E ^ D0) / kross;
if ((t < 0.0) || (t > 1.0))
{
// Intersection of lines is not a point on segment s2p1 + t * D1
return false;
}
// Intersection of lines is a point on each segment
return true;
}
// Lines of the segments are parallel
const double sqrLenE= E.x() * E.x() + E.y() * E.y();
kross= E ^ D0;
sqrKross= kross * kross;
if (sqrKross > (EPSILON * sqrLen0 * sqrLenE))
{
// Lines of the segments are different
return false;
}
// Lines of the segments are the same. Need to test for overlap of segments.
const double s0= (D0 * E) / sqrLen0;
const double s1= s0 + (D0 * D1) / sqrLen0;
const double sMin= qMin(s0, s1);
const double sMax= qMax(s0, s1);
if (findIntersection(0.0, 1.0, sMin, sMax).size() == 0) return false; else return true;
}
bool LineSegment::colinear(const LineSegment& l) const // lines are colinear if their slopes are the same AND if there is a point they both pass through
{
if(l.slope() != slope()) return false;
// assert: lines are the same slope
LineSegment buffer1(l); // to preserve const
LineSegment buffer2(*this);
if( findIntersection(buffer1, buffer2, true) == noCoord) // check for a point lying on both lines
return false;
// assert: we found a point that lies on both lines
return true;
}
C2dImagePointPx C2dLine::closestPointToBoundedLine(const C2dBoundedLine & boundedLine) const
{
//return closestPointToBoundedLine_int<C2dBoundedLine>(boundedLine);
optional<const C2dImagePointPx> intersection = findIntersection(boundedLine.unboundedLine());
if(intersection)
{
//If intersection is on the bounded line, return intersection, else return whichever point on this line is closest to the bounded line endpoint
C2dImagePointPx closestPointOnBounded = boundedLine.closestPoint(*intersection);
return closestPoint(closestPointOnBounded);
}
else
{
return getPointOnLine();
}
}
static bool findLineSegmentIntersection(const FloatPointGraph::Edge& edgeA, const FloatPointGraph::Edge& edgeB, FloatPoint& intersectionPoint)
{
if (!findIntersection(*edgeA.first, *edgeA.second, *edgeB.first, *edgeB.second, intersectionPoint))
return false;
FloatPoint edgeAVec(*edgeA.second - *edgeA.first);
FloatPoint edgeBVec(*edgeB.second - *edgeB.first);
float dotA = edgeAVec.dot(toFloatPoint(intersectionPoint - *edgeA.first));
if (dotA < 0 || dotA > edgeAVec.lengthSquared())
return false;
float dotB = edgeBVec.dot(toFloatPoint(intersectionPoint - *edgeB.first));
if (dotB < 0 || dotB > edgeBVec.lengthSquared())
return false;
return true;
}
Coord findIntersection(LineSegment& one, LineSegment& two, double extrapolatePercentage)
{
// extrapolate both lines out by making new segments that are larger
one.normalize(); two.normalize(); // A < B
// line one
double ext1 = extrapolatePercentage*one.length();
Coord A1 (one.A.x-ext1, one.A.y-ext1*one.slope());
Coord B1 (one.B.x+ext1, one.B.y+ext1*one.slope());
LineSegment L1 (A1, B1);
// line one
double ext2 = extrapolatePercentage*two.length();
Coord A2 (two.A.x-ext2, two.A.y-ext2*two.slope());
Coord B2 (two.B.x+ext2, two.B.y+ext2*two.slope());
LineSegment L2 (A2, B2);
// lines are now ready for intersection check
return findIntersection(L1, L2, false);
}
void JLinkage::selectMinimalSets(const std::vector<Line>& edges)
{
srand(time(NULL)); // Seed the rand function
models.clear();
models.resize(JLINKAGE_MODEL_SIZE);
for(size_t i = 0; i < JLINKAGE_MODEL_SIZE; ++i)
{
Model& m = models[i];
// For each modelsM choose, randomly, two lines
size_t n1 = rand()%line_count;
size_t n2 = rand()%line_count;
while(n2 == n1)
n2 = rand()%line_count;
m.line1_idx = n1;
m.line2_idx = n2;
// Find vanishing point (intersection) for chosen model
m.intersection_pt = findIntersection(edges[n1], edges[n2]);
}
}
void Shape::drawTextured(ShadowBuffer& sb, Point const textureAnchor, int textureWidth, int textureHeight, Color ** textureCache) {
Util util;
Point p1, p2;
Point * tipPoints = getTipPoints();
for(int i = tipPoints[0].y; i <= tipPoints[1].y; i++) {
vector<Point> ListOfIntersectPoints;
for(int j = 0; j < points.size(); j++) {
if (j != (points.size() - 1)) {
p1 = points[j];
p2 = points[j+1];
} else {
p1 = points[j];
p2 = points[0];
}
int intersectX;
if (findIntersection(p1,p2,i,intersectX)) {
if(p1.y > p2.y) {
std::swap(p1,p2);
}
if (i != p2.y) {
Point intersect(intersectX, i, 0);
ListOfIntersectPoints.push_back(intersect);
}
}
}
vector<Point> sortedResult = sortVector(ListOfIntersectPoints);
int intersectPointsSize = sortedResult.size();
Color d(225, 0, 0);
for(int j = 0; j < intersectPointsSize-1; j+=2) {
Line line(sortedResult[j], sortedResult[j + 1]);
line.drawTextured(sb, textureAnchor, textureWidth, textureHeight, textureCache);
}
}
delete [] tipPoints;
}
static vec3 calcMaterialColor(materialT* material, raytracerT* raytracer, intersectionT* intersection) {
diffuseMaterialT* difmat = material->data;
vec3 color = difmat->ambient_color;
depth++;
lightSourceT* light_source = raytracer->light_sources;
while (light_source) {
float mult = 1.0f / light_source->num_samples;
for (int i = 0; i < light_source->num_samples; i++) {
lightRayT light_ray = light_source->light_fn(light_source, intersection);
rayT shadow_ray;
shadow_ray.origin = intersection->position;
shadow_ray.direction = light_ray.direction;
//vec_flip(&shadow_ray.direction, &shadow_ray.direction);
intersectionT occlusion_intersection = findIntersection(raytracer, &shadow_ray, intersection->surface, light_ray.distance);
if (occlusion_intersection.t <= 0.0f) {
float f = vec_dot(&intersection->normal, &light_ray.direction);
f *= light_ray.intensity;
f = clamp(f, 0.0f, 1.0f);
color.x += f*difmat->diffuse_color.x*mult;
color.y += f*difmat->diffuse_color.y*mult;
color.z += f*difmat->diffuse_color.z*mult;
}
}
light_source = light_source->next;
}
vec3 c3 = { 0 };
rayT ray = { 0 };
if (depth < 2) {
ray.origin = intersection->position;
int num_samples = 4;
for (int i = 0; i < num_samples; i++) {
//vec_reflect(&intersection->ray.direction, &intersection->normal, &ray.direction);
ray.direction = intersection->normal;
float x = 1.8f*((rand() / (float)RAND_MAX) - 0.5f);
float y = 1.8f*((rand() / (float)RAND_MAX) - 0.5f);
float z = 1.8f*((rand() / (float)RAND_MAX) - 0.5f);
ray.direction.x += x;
ray.direction.y += y;
ray.direction.z += z;
vec_normalize(&ray.direction, &ray.direction);
intersectionT intersection2 = findIntersection(raytracer, &ray, intersection->surface, FLT_MAX);
if (intersection2.t > 0.0f) {
vec3 c2 = calcFinalColor(raytracer, &intersection2);
vec_add(&c3, &c2, &c3);
}
}
vec_scale(&c3, 1.0f / num_samples, &color);
color.x = color.x * 0.98f + c3.x * 0.02f;
color.y = color.y * 0.98f + c3.y * 0.02f;
color.z = color.z * 0.98f + c3.z * 0.02f;
}
depth--;
return (color);
}
void Shape::scanLineFill(ShadowBuffer& sb, vector<Point> v) {
Util util;
Point p1, p2;
int edgesSize = v.size();
Point * tipPoints = getTipPoints();
// vector<map<int,int> > brensenham;
// for(int i=1; i < v.size(); i++){
// if(v[i-1].y!=v[i].y){
// Line l(v[i-1], v[i]);
// brensenham.push_back(l.getLinePoints());
// }
// }
// Line l(v[v.size()-1], v[0]);
// brensenham.push_back(l.getLinePoints());
for(int i = tipPoints[0].y; i <= tipPoints[1].y; i++) {
vector<Point> ListOfIntersectPoints;
for(int j = 0; j < edgesSize; j++) {
if (j != (edgesSize - 1)) {
p1 = v[j];
p2 = v[j+1];
} else {
p1 = v[j];
p2 = v[0];
}
int intersectX;
// if((floatToInt(p1.y)==floatToInt(p2.y))&&(floatToInt(p1.x)==floatToInt(p2.x))&&(floatToInt(p1.y)==i)){
// Point intersect(floatToInt(p1.x), i, 0);
// ListOfIntersectPoints.push_back(intersect);
// }
if (findIntersection(p1,p2,i,intersectX)) {
if(p1.y > p2.y) {
std::swap(p1,p2);
}
if (i != p2.y) {
Point intersect(intersectX, i, 0);
ListOfIntersectPoints.push_back(intersect);
}
}
}
// for(int j=0; j < brensenham.size(); j++){
// try{
// if(brensenham[j][i]!=0)
// ListOfIntersectPoints.push_back(Point(brensenham[j][i], i, 0));
// }catch(const std::out_of_range& oor) {
// }
// }
vector<Point> result = sortVector(ListOfIntersectPoints);
int intersectPointsSize = result.size();
Color d(225, 0, 0);
/*if(result.size()>0 ){
if (i >= 400 && i <= 410) {
cout<<"y: "<<i<<endl<< "nilai x:";
for(int k=0; k< intersectPointsSize; k++) {
cout << result[k].x<< "--"; //<< " , "<< result[k+1].x << " ";
}
cout << endl;
// for (int k = 0; k < ListOfIntersectPoints.size(); k++) {
// cout << ListOfIntersectPoints[k].x << "--";
// }
// cout << endl;
}
}*/
for(int j = 0; j < intersectPointsSize-1; j+=2) {
Line line(result[j], result[j + 1]);
line.color = this->color;
line.draw(sb);
//line.draw(sb, line.getPoint1(), line.getPoint1().getDistance(line.getPoint2()), line.color, Color(line.color.r - 50, line.color.g - 50, line.color.b - 50));
//line.drawTextured(sb, basePoint, textureWidth, textureHeight, textureCache);
}
}
delete [] tipPoints;
}
void polygonClipping(face_info* face){
int i=0;
vertex* verts=face->vertex_set;
int count=face->number_of_vertices;
std::vector<vertex*> CvTable;
std::vector<vertex*> CvTabletemp;
for(i=0;i<count;i++){
vertex* point=verts+i;
CvTable.push_back(point);
}
for(i=0;i<clipping_plane_eq.size();i++)
{
float* eq_plane=clipping_plane_eq.at(i);
//printf("equation of plane %d %f %f %f %f \n",i,eq_plane[0],eq_plane[1],eq_plane[2],eq_plane[3]);
int j;
for(j=0;j<CvTable.size();j++)
{
vertex* point1=CvTable.at(j);
vertex* point2=CvTable.at((j+1)%CvTable.size());
vertex* unitvect=unitVector(point1,point2);
Ray* ray=(Ray*)malloc(sizeof(Ray));
ray->direction=unitvect;
ray->startPoint=point1;
//printf("point1=%f %f %f\n",point1->x_pos,point1->y_pos,point1->z_pos);
//printf("point2=%f %f %f\n",point2->x_pos,point2->y_pos,point2->z_pos);
int k;
if(isInsidePlane(eq_plane,point1)) //if v1 is inside
{
if(isInsidePlane(eq_plane,point2)) //if v2 is inside
{
//printf("1\n");
CvTabletemp.push_back(point2);
}
else //if v2 is outside
{
//find intersection point and put in cvtable
vertex* temp=findIntersection(eq_plane,ray);
if(temp!=NULL)
CvTabletemp.push_back(temp);
}
}
else //if v1 is outside
{
if(isInsidePlane(eq_plane,point2)) //if v2 is inside
{
//find intersection point and put in cvtable
vertex* temp=findIntersection(eq_plane,ray);
if(temp!=NULL)
CvTabletemp.push_back(temp);
CvTabletemp.push_back(point2);
}
}
}
CvTable.clear();
int k;
for(k=0;k<CvTabletemp.size();k++){
CvTable.push_back(CvTabletemp.at(k));
}
CvTabletemp.clear();
}
vertex* newverts= (vertex*)malloc(CvTable.size()*sizeof(vertex));
for(i=0;i<CvTable.size();i++){
newverts[i]=CvTable.at(i)[0];
}
face->vertex_set=newverts;
face->number_of_vertices=CvTable.size();
}
void Shape::scanLineIntersect(ShadowBuffer& sb, Shape available) {
Point p1, p2;
int nAvailable = available.points.size();
int nDemand = points.size();
int a = 0;
available.draw(sb);
Point * tipPoints = available.getTipPoints();
//cout<< "ymin : "<<tipPoints[0].y<< " ymax : "<<tipPoints[1].y<<endl;
for (int i = tipPoints[0].y; i <= tipPoints[1].y; i++) {
//Line line(Point(1,i,0),Point(500,i,0));
// line.color= Color(225,0,0);
//line.draw(sb);
//for (int k = 0; k < nAvailable; k++) {
vector<Point> ListOfIntersectPoints;
//Shape tempShape = pointToPrint[k];
int edgesSize = available.points.size();
//Color c = Color(tempShape.points[edgesSize-1].x,tempShape.points[edgesSize-1].y + 180 - (int)(i / 2),tempShape.points[edgesSize-1].z);
for (int j = 0; j < edgesSize ; j++) {
//cout<< "GARIS ke-"<<j<<endl;
if (j != (edgesSize - 1)) {
p1 = available.points[j];
p2 = available.points[j+1];
} else {
p1 = available.points[j];
p2 = available.points[0];
}
int intersectX;
if (findIntersection(p1,p2,i,intersectX)){
//cout<< intersectX<<endl;
if (p1.y > p2.y) {
std::swap(p1,p2);
}
Point intersect(intersectX, i,0);
if (intersect.y != p2.y)
ListOfIntersectPoints.push_back(intersect);
}
}
vector<Point> sort = sortVector(ListOfIntersectPoints);
//cout<<"setelah sort"<<endl;
// for(int i=0; i<sort.size(); i++){
// //cout<< "x, y =" << sort[i].x << ", " << sort[i].y<< endl;
// }
if(sort.size()>0) {
vector<Line> resultAvailable = initAvailable(sort,sb);
vector<Point> ListOfIntersectPoints2;
int edgesSize2 = points.size();
for (int j = 0; j < (edgesSize2 ); j++) {
if (j != (edgesSize2 - 1)) {
p1 = points[j];
p2 = points[j+1];
} else {
p1 = points[j];
p2 = points[0];
}
int intersectX;
if (findIntersection(p1,p2,i,intersectX)){
if (p1.y > p2.y) {
std::swap(p1,p2);
}
Point intersect(intersectX, i,0);
if (intersect.y == p2.y) continue;
ListOfIntersectPoints2.push_back(intersect);
}
}
vector<Point> resultDemand = sortVector(ListOfIntersectPoints2);
drawAvailable(resultAvailable, resultDemand,sb, this->color);
}
//}
}
delete [] tipPoints;
}
void PIDuser::calcTurnSignal(std::vector<float> &state, float &turn)
{
m_refInd = findIntersection(state, m_startInd);
turn = calcTurnSignal(state, m_refInd);
}
