static int check_linear(const Cubic& cubic, Cubic& reduction,
int minX, int maxX, int minY, int maxY) {
int startIndex = 0;
int endIndex = 3;
while (cubic[startIndex].approximatelyEqual(cubic[endIndex])) {
--endIndex;
if (endIndex == 0) {
printf("%s shouldn't get here if all four points are about equal", __FUNCTION__);
assert(0);
}
}
if (!isLinear(cubic, startIndex, endIndex)) {
return 0;
}
// four are colinear: return line formed by outside
reduction[0] = cubic[0];
reduction[1] = cubic[3];
int sameSide1;
int sameSide2;
bool useX = cubic[maxX].x - cubic[minX].x >= cubic[maxY].y - cubic[minY].y;
if (useX) {
sameSide1 = sign(cubic[0].x - cubic[1].x) + sign(cubic[3].x - cubic[1].x);
sameSide2 = sign(cubic[0].x - cubic[2].x) + sign(cubic[3].x - cubic[2].x);
} else {
sameSide1 = sign(cubic[0].y - cubic[1].y) + sign(cubic[3].y - cubic[1].y);
sameSide2 = sign(cubic[0].y - cubic[2].y) + sign(cubic[3].y - cubic[2].y);
}
if (sameSide1 == sameSide2 && (sameSide1 & 3) != 2) {
return 2;
}
double tValues[2];
int roots;
if (useX) {
roots = findExtrema(cubic[0].x, cubic[1].x, cubic[2].x, cubic[3].x, tValues);
} else {
roots = findExtrema(cubic[0].y, cubic[1].y, cubic[2].y, cubic[3].y, tValues);
}
for (int index = 0; index < roots; ++index) {
_Point extrema;
extrema.x = interp_cubic_coords(&cubic[0].x, tValues[index]);
extrema.y = interp_cubic_coords(&cubic[0].y, tValues[index]);
// sameSide > 0 means mid is smaller than either [0] or [3], so replace smaller
int replace;
if (useX) {
if (extrema.x < cubic[0].x ^ extrema.x < cubic[3].x) {
continue;
}
replace = (extrema.x < cubic[0].x | extrema.x < cubic[3].x)
^ cubic[0].x < cubic[3].x;
} else {
if (extrema.y < cubic[0].y ^ extrema.y < cubic[3].y) {
continue;
}
replace = (extrema.y < cubic[0].y | extrema.y < cubic[3].y)
^ cubic[0].y < cubic[3].y;
}
reduction[replace] = extrema;
}
return 2;
}
static int check_linear(const Quadratic& quad, Quadratic& reduction,
int minX, int maxX, int minY, int maxY) {
int startIndex = 0;
int endIndex = 2;
while (quad[startIndex].approximatelyEqual(quad[endIndex])) {
--endIndex;
if (endIndex == 0) {
printf("%s shouldn't get here if all four points are about equal", __FUNCTION__);
assert(0);
}
}
if (!isLinear(quad, startIndex, endIndex)) {
return 0;
}
// four are colinear: return line formed by outside
reduction[0] = quad[0];
reduction[1] = quad[2];
int sameSide;
bool useX = quad[maxX].x - quad[minX].x >= quad[maxY].y - quad[minY].y;
if (useX) {
sameSide = sign(quad[0].x - quad[1].x) + sign(quad[2].x - quad[1].x);
} else {
sameSide = sign(quad[0].y - quad[1].y) + sign(quad[2].y - quad[1].y);
}
if ((sameSide & 3) != 2) {
return 2;
}
double tValue;
int root;
if (useX) {
root = findExtrema(quad[0].x, quad[1].x, quad[2].x, &tValue);
} else {
root = findExtrema(quad[0].y, quad[1].y, quad[2].y, &tValue);
}
if (root) {
_Point extrema;
extrema.x = interp_quad_coords(quad[0].x, quad[1].x, quad[2].x, tValue);
extrema.y = interp_quad_coords(quad[0].x, quad[1].x, quad[2].x, tValue);
// sameSide > 0 means mid is smaller than either [0] or [2], so replace smaller
int replace;
if (useX) {
if (extrema.x < quad[0].x ^ extrema.x < quad[2].x) {
return 2;
}
replace = (extrema.x < quad[0].x | extrema.x < quad[2].x)
^ (quad[0].x < quad[2].x);
} else {
if (extrema.y < quad[0].y ^ extrema.y < quad[2].y) {
return 2;
}
replace = (extrema.y < quad[0].y | extrema.y < quad[2].y)
^ (quad[0].y < quad[2].y);
}
reduction[replace] = extrema;
}
return 2;
}
Node_ptr Array<T>::getNode() const {
if (node->isBuffer()) {
BufferNode<T> *bufNode = reinterpret_cast<BufferNode<T> *>(node.get());
unsigned bytes = this->getDataDims().elements() * sizeof(T);
bufNode->setData(data, bytes, getOffset(), dims().get(),
strides().get(), isLinear());
}
return node;
}
/// Un-split any def on this edge, if its type is the same as the value
/// going into the IF, except EFFECT types.
///
Def* IdentityAnalyzer::do_arm(ArmInstr* arm) {
if (isLinear(type(def_))) {
// Do not un-split linear types, so as to avoid creating scheduling
// headaches later by allowing uses to float out of basic blocks.
return def_;
}
Def* arg = def(arm->owner->arg(pos(def_)));
if (*type(def_) == *type(arg))
return arg;
return def_;
}
void Face::computeCachedNormal()
{
// note: this face might be non-planar, so average the two triangle normals
Vec3f a = (*this)[0]->get();
Vec3f b = (*this)[1]->get();
Vec3f c = (*this)[2]->get();
Vec3f d = (*this)[3]->get();
//Checks to make sure we don't have three points that we are using here to check the normal be linear -- that could cause errors!!
if(isLinear(a,b,c)){
cached_normal = new Vec3f(ComputeNormal(a,c,d));
return;
}
if(isLinear(a,c,d)){
cached_normal = new Vec3f(ComputeNormal(a,b,c));
return;
}
cached_normal = new Vec3f(0.5 * (ComputeNormal(a,b,c) + ComputeNormal(a,c,d)));
return;
}
void Group<E>::linearize() {
if (isLinear()) {
return;
}
for(std::size_t i = 0; i < this->size(); i++) {
if (!this->get(i)->isLineal()) {
this->set(i, this->get(i)->linearize());
}
}
}
float
Plan::getExecutionDuration(void) const
{
if (!isLinear() || !m_profiles->size())
return -1.0;
const std::string& str_last = m_profiles->lastValid();
if (str_last.empty())
return -1.0;
TimeProfile::const_iterator itr = m_profiles->find(str_last);
if (itr == m_profiles->end())
return -1.0;
return itr->second.durations.back();
}
void StringData::dump() {
const char *p = data();
int len = size();
printf("StringData(%d) (%s%s%s%d): [", _count,
isLiteral() ? "literal " : "",
isShared() ? "shared " : "",
isLinear() ? "linear " : "",
len);
for (int i = 0; i < len; i++) {
char ch = p[i];
if (isprint(ch)) {
std::cout << ch;
} else {
printf("\\%02x", ch);
}
}
printf("]\n");
}
void StringData::dump() const {
const char *p = data();
int len = size();
printf("StringData(%d) (%s%s%s%s%d): [", _count,
isLiteral() ? "literal " : "",
isShared() ? "shared " : "",
isLinear() ? "linear " : "",
isStatic() ? "static " : "",
len);
for (int i = 0; i < len; i++) {
char ch = p[i];
if (isprint(ch)) {
std::cout << ch;
} else {
printf("\\x%02x", ch);
}
}
#ifdef TAINTED
printf("\n");
this->getTaintDataRef().dump();
#endif
printf("]\n");
}
std::string LogicInfo::getLogicString() const {
CheckArgument(d_locked, *this, "This LogicInfo isn't locked yet, and cannot be queried");
if(d_logicString == "") {
LogicInfo qf_all_supported;
qf_all_supported.disableQuantifiers();
qf_all_supported.lock();
if(hasEverything()) {
d_logicString = "ALL_SUPPORTED";
} else if(*this == qf_all_supported) {
d_logicString = "QF_ALL_SUPPORTED";
} else {
size_t seen = 0; // make sure we support all the active theories
stringstream ss;
if(!isQuantified()) {
ss << "QF_";
}
if(d_theories[THEORY_ARRAY]) {
ss << (d_sharingTheories == 1 ? "AX" : "A");
++seen;
}
if(d_theories[THEORY_UF]) {
ss << "UF";
++seen;
}
if(d_theories[THEORY_BV]) {
ss << "BV";
++seen;
}
if(d_theories[THEORY_DATATYPES]) {
ss << "DT";
++seen;
}
if(d_theories[THEORY_ARITH]) {
if(isDifferenceLogic()) {
ss << (areIntegersUsed() ? "I" : "");
ss << (areRealsUsed() ? "R" : "");
ss << "DL";
} else {
ss << (isLinear() ? "L" : "N");
ss << (areIntegersUsed() ? "I" : "");
ss << (areRealsUsed() ? "R" : "");
ss << "A";
}
++seen;
}
if(seen != d_sharingTheories) {
Unhandled("can't extract a logic string from LogicInfo; at least one "
"active theory is unknown to LogicInfo::getLogicString() !");
}
if(seen == 0) {
ss << "SAT";
}
d_logicString = ss.str();
}
}
return d_logicString;
}
float
Plan::progress(const IMC::ManeuverControlState* mcs)
{
if (!m_compute_progress)
return -1.0;
// Compute only if linear and durations exists
if (!isLinear() || !m_profiles->size())
return -1.0;
// If calibration has not started yet, but will later
if (m_calib->notStarted())
return -1.0;
float total_duration = getTotalDuration();
float exec_duration = getExecutionDuration();
// Check if its calibrating
if (m_calib->inProgress())
{
float time_left = m_calib->getRemaining() + exec_duration;
m_progress = 100.0 * trimValue(1.0 - time_left / total_duration, 0.0, 1.0);
return m_progress;
}
// If it's not executing, do not compute
if (mcs->state != IMC::ManeuverControlState::MCS_EXECUTING ||
mcs->eta == 0)
return m_progress;
TimeProfile::const_iterator itr;
itr = m_profiles->find(getCurrentId());
// If not found
if (itr == m_profiles->end())
{
// If beyond the last maneuver with valid duration
if (m_beyond_dur)
{
m_progress = 100.0;
return m_progress;
}
else
{
return -1.0;
}
}
// If durations vector for this maneuver is empty
if (!itr->second.durations.size())
return m_progress;
IMC::Message* man = m_graph.find(getCurrentId())->second.pman->data.get();
// Get execution progress
float exec_prog = Progress::compute(man, mcs, itr->second.durations, exec_duration);
float prog = 100.0 - getExecutionPercentage() * (1.0 - exec_prog / 100.0);
// If negative, then unable to compute
// But keep last value of progress if it is not invalid
if (prog < 0.0)
{
if (m_progress < 0.0)
return -1.0;
else
return m_progress;
}
// Never output shorter than previous
m_progress = prog > m_progress ? prog : m_progress;
return m_progress;
}
void
Plan::secondaryParse(const std::map<std::string, IMC::EntityInfo>& cinfo,
IMC::PlanStatistics& ps, bool imu_enabled,
const IMC::EstimatedState* state)
{
// Pre statistics
ps.plan_id = m_spec->plan_id;
PreStatistics pre_stat(&ps);
if (m_compute_progress)
{
sequenceNodes();
if (isLinear() && state != NULL)
{
m_profiles->parse(m_seq_nodes, state);
Timeline tline;
fillTimeline(tline);
Memory::clear(m_sched);
m_sched = new ActionSchedule(m_task, m_spec, m_seq_nodes,
tline, cinfo);
// Update timeline with scheduled calibration time if any
tline.setPlanETA(std::max(m_sched->getEarliestSchedule(), getExecutionDuration()));
// Fill component active time with action scheduler
m_sched->fillComponentActiveTime(m_seq_nodes, tline, m_cat);
// Update duration statistics
pre_stat.fill(m_seq_nodes, tline);
// Update action statistics
pre_stat.fill(m_cat);
// Estimate necessary calibration time
float diff = m_sched->getEarliestSchedule() - getExecutionDuration();
m_est_cal_time = (uint16_t)std::max(0.0f, diff);
m_est_cal_time = (uint16_t)std::max(m_min_cal_time, m_est_cal_time);
if (m_predict_fuel)
{
Memory::clear(m_fpred);
m_fpred = new FuelPrediction(m_profiles, &m_cat, m_power_model,
m_speed_model, imu_enabled, tline.getPlanETA());
pre_stat.fill(*m_fpred);
}
}
else if (!isLinear())
{
Memory::clear(m_sched);
m_sched = new ActionSchedule(m_task, m_spec, m_seq_nodes, cinfo);
m_est_cal_time = m_min_cal_time;
}
}
if (!m_profiles->isDurationFinite())
m_properties |= IMC::PlanStatistics::PRP_INFINITE;
pre_stat.setProperties(m_properties);
}
//===========================================================================
void Intersector::compute(bool compute_at_boundary)
//===========================================================================
{
// Purpose: Compute the topology of the current intersection
// Make sure that no "dead intersection points" exist in the pool,
// i.e. points that have been removed when compute() has been run
// on sibling subintersectors.
int_results_->synchronizePool();
// Make sure that all intersection points at the
// boundary/boundaries of the current object are already computed
if (compute_at_boundary)
getBoundaryIntersections();
// Remove inner points in constant parameter intersection
// links
// (vsk, 0609) and isolated points identical to the existing ones
int_results_->cleanUpPool();
int nmb_orig = int_results_->numIntersectionPoints();
if (getenv("DEBUG") && *(getenv("DEBUG")) == '1') {
try {
printDebugInfo();
} catch (...) {
MESSAGE("Failed printing debug info, continuing.");
}
}
// Check if any intersections are possible in the inner of the
// objects
int status_intercept = performInterception();
// Branch on the outcome of the interseption test
if (status_intercept == 0) {
// No intersection is possible
} else if (status_intercept == 2) {
// Both objects are too small for further processing.
// Handle micro case
microCase();
} else if (degTriangleSimple()) {
// This situation is currently relevant only for intersections
// between two parametric surfaces. It will probably at some
// stage be relevant for two-parametric functions.
// All the necessary connections are made
} else if (checkCoincidence()) {
// The two objects coincide. The representation is already
// updated according to this situation
} else {
// status_intercept == 1
// Intersections might exist. Check for simple case. 0 = Maybe
// simple case; 1 = Confirmed simple case.
int status_simplecase = simpleCase();
if (status_simplecase == 1) {
// Confirmed simple case.
// Compute intersection points or curves according to the
// properties of this particular intersection
updateIntersections();
} else if (isLinear()) {
// Linearity is a simple case, but it is important to
// check for coincidence before trying to find/connect
// intersections as the simple case criteria is not
// satisfied
updateIntersections();
}
else if (complexIntercept())
{
// Interception by more complex algorithms is performed
// (implicitization). No further intersectsions are found
// to be possible
}
else if (complexSimpleCase())
{
// Simple case test by more complex algorithms is performed
// (implicitization). A simple case is found.
updateIntersections();
} else if (!complexityReduced()) {
// For the time being, write documentation of the
// situation to a file
handleComplexity();
} else {
// It is necessary to subdivide the current objects
doSubdivide();
int nsubint = int(sub_intersectors_.size());
for (int ki = 0; ki < nsubint; ki++) {
sub_intersectors_[ki]->getIntPool()
->includeCoveredNeighbourPoints();
sub_intersectors_[ki]->compute();
}
}
}
// // Write intersection point diagnostics
// if (numParams() == 4) {
// writeIntersectionPoints();
// }
if (prev_intersector_ && prev_intersector_->numParams() > numParams())
{
// Remove inner points in constant parameter intersection
// links
// (vsk, 0609) and isolated points identical to the existing ones
int_results_->cleanUpPool(nmb_orig);
// No more recursion at this level. Post iterate the intersection points
doPostIterate();
}
// Prepare output intersection results
if (prev_intersector_ == 0 || prev_intersector_->isSelfIntersection())
{
/*if (getenv("DEBUG_FINISH") && *(getenv("DEBUG_FINISH")) == '1') {
cout << "Status after cleaning up pool:" << endl;
writeIntersectionPoints();
}*/
// Remove loose ends of intersection links in the inner
//int_results_->weedOutClutterPoints();
if (getenv("DEBUG_FINISH") && *(getenv("DEBUG_FINISH")) == '1')
{
cout << "Status after removing clutter points:" << endl;
writeIntersectionPoints();
int_results_->writeDebug();
}
// Remove loose ends of intersection links in the inner
//int_results_->weedOutClutterPoints();
int_results_->cleanUpPool(0);
if (true /*getenv("DO_REPAIR") && *(getenv("DO_REPAIR")) == '1'*/)
{
if (getenv("DEBUG_FINISH") && *(getenv("DEBUG_FINISH")) == '1')
{
cout << "Starting repair" << endl;
}
repairIntersections();
if (getenv("DEBUG_FINISH") && *(getenv("DEBUG_FINISH")) == '1')
{
cout << "Status after repairing intersections:" << endl;
writeIntersectionPoints();
}
}
}
if (prev_intersector_ == 0) {
// Top level intersector
/*if (getenv("DEBUG_FINISH") && *(getenv("DEBUG_FINISH")) == '1') {
cout << "Status after removing clutter points:" << endl;
writeIntersectionPoints();
}*/
// if (/*true */getenv("DO_REPAIR") && *(getenv("DO_REPAIR")) == '1') {
// repairIntersections();
// if (getenv("DEBUG_FINISH") && *(getenv("DEBUG_FINISH")) == '1') {
// cout << "Status after repairing intersections:" << endl;
// writeIntersectionPoints();
// }
// }
if (getenv("DEBUG_FINISH") && *(getenv("DEBUG_FINISH")) == '1') {
int_results_->writeDebug();
}
int_results_->makeIntersectionCurves();
}
}
