int main() {
const uint32_t samples_per_pixel = 16;
const uint32_t width = 1280;
const uint32_t height = 720;
const vec3 cam_pos(0, 0, 25);
const vec3 look_at(0, 0, -1);
const vec3 up(0, 1, 0);
camera cam(cam_pos, look_at, up, 90, (float)width / height);
image img(width, height);
sphere s(vec3(0, 0, -0.5), 1);
vec3 light_dir = normalize(vec3(-1, -1, -1));
vec3 ambient_color = vec3(1.0, 0, 0);
vec3 *pixels = img.pixels;
for (uint32_t y = 0; y < height; ++y) {
for (uint32_t x = 0; x < width; ++x) {
vec3 color(0, 0, 0);
for (uint32_t sample = 0; sample < samples_per_pixel; ++sample) {
const ray r = make_ray(
cam,
(x + rand_float(generator)) / width,
(height - (y + rand_float(generator))) / height
);
vec3 hit;
vec3 normal;
if (intersect(s, r, &hit, &normal)) {
vec3 dir_light_color = fmaxf(0, dot(normal, -light_dir)) * vec3(1.0, 1.0, 1.0);
color += saturate(ambient_color + dir_light_color);
}
}
color /= samples_per_pixel;
*pixels++ = color;
}
}
save_as_png(img, "test.png");
return 0;
}
/**
* Determines the worst resulting aspect ratio of a set of rectangles laid out in a larger one.
*
* @param sum The sum of all areas in the list.
* @param min_dim The value of the outer rectangle's minimum dimension.
* @param areas The areas to be laid out.
* @param num_rects The number of rectangles in the list.
*/
float worst(float sum, float min_dim, float *areas, size_t num_rects) {
float sum_sq = sum * sum;
float min_dim_sq = min_dim * min_dim;
float min_area = areas[0];
float max_area = areas[0];
size_t i;
/* Determine the rectangles with the largest and smallest areas */
for(i = 1; i < num_rects; ++i) {
if(areas[i] < min_area) {
min_area = areas[i];
} else if(areas[i] > max_area) {
max_area = areas[i];
}
}
return fmaxf(min_dim_sq * max_area / sum_sq, sum_sq / (min_dim_sq * min_area));
}
/* shoot on circle */
uint8_t dc_circle(float interval, float start) {
dc_autoshoot = DC_AUTOSHOOT_CIRCLE;
dc_gps_count = 0;
dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);
if(start == DC_IGNORE) {
start = DegOfRad(estimator_psi);
}
dc_circle_start_angle = fmodf(start, 360.);
if (start < 0.)
start += 360.;
//dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
dc_circle_last_block = 0;
dc_circle_max_blocks = floorf(360./dc_circle_interval);
dc_probing = TRUE;
dc_info();
return 0;
}
void Camera::setCameraOrbitLocation(float yawDegrees, float pitchDegrees, float radius)
{
m_cameraOrbitYawDegrees= wrap_degrees(yawDegrees);
m_cameraOrbitPitchDegrees= clampf(pitchDegrees, 0.f, 60.f);
m_cameraOrbitRadius= fmaxf(radius, k_camera_min_zoom);
const float yawRadians= degrees_to_radians(m_cameraOrbitYawDegrees);
const float pitchRadians= degrees_to_radians(m_cameraOrbitPitchDegrees);
const float xzRadiusAtPitch= m_cameraOrbitRadius*cosf(pitchRadians);
m_cameraPosition=
glm::vec3(
xzRadiusAtPitch*sinf(yawRadians),
m_cameraOrbitRadius*sinf(pitchRadians),
xzRadiusAtPitch*cosf(yawRadians))
+ glm::vec3(0.f, k_camera_y_offset, 0.f);
publishCameraViewMatrix();
}
void graphModel::setRange( float _min, float _max )
{
if( _min != m_minValue || _max != m_maxValue )
{
m_minValue = _min;
m_maxValue = _max;
if( !m_samples.isEmpty() )
{
// Trim existing values
for( int i=0; i < length(); i++ )
{
m_samples[i] = fmaxf( _min, fminf( m_samples[i], _max ) );
}
}
emit rangeChanged();
}
}
/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
* Output: '_att_control' vector
*/
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _ctrl_state.roll_rate;
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* throttle pid attenuation factor */
float tpa = fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint)));
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
_params.rate_ff.emult(_rates_sp);
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
/* update integral only if not saturated on low limit and if motor commands are not saturated */
if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT &&
/* if the axis is the yaw axis, do not update the integral if the limit is hit */
!((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) {
_rates_int(i) = rate_i;
}
}
}
}
}
float ArmClass::Validate_Turret_Command(float cmd, bool ispidcmd)
{
#if 01
int tur = GetTurretEncoder();
if (cmd >= 0.0f) // if cmd is moving turret toward lower angle...
{
if (tur < ARM_TURRET_MIN_ENCODER) // and it is past the lowest angle allowed
{
//float error = ARM_TURRET_MIN_ENCODER - tur;
// BAD!
return 0.0f;
}
}
else if (cmd <= 0.0f) // if cmd is moving turret toward higher angles...
{
if (tur > ARM_TURRET_MAX_ENCODER) // and it is past the highest angle allowed
{
// BAD!
return 0.0f;
}
}
#endif
if(ispidcmd)
{
//randon .002 commands...
if(cmd > 0.003f)
{
cmd = fmaxf(cmd, MIN_TURRET_CMD);
}
else if (cmd < -.003f)
{
cmd = fminf(cmd, -MIN_TURRET_CMD);
}
else if (cmd < .003f && cmd > -.003f)
{
cmd = 0.0f;
}
}
return cmd;
}
glm::vec3 BlinnMicrofacetBxDF::SampleAndEvaluateScatteredEnergy(const glm::vec3 &wo, glm::vec3 &wi_ret, float rand1, float rand2, float &pdf_ret) const{
float cos_theta( powf( rand1, 1.f / ( exponent + 1.f ) ) );
float sin_theta( sqrtf( fmaxf( 0.f, 1.f - cos_theta * cos_theta ) ) );
float phi( rand2 * TWO_PI );
float cos_phi( cosf( phi ) );
float sin_phi( sinf( phi ) );
glm::vec3 wh( sin_theta * cos_phi, sin_theta * sin_phi, cos_theta );
float woDwh( glm::dot( wo, wh ) );
if( woDwh < 0.f ) wh = -wh;
wi_ret = -wo + 2.f * woDwh * wh;
pdf_ret = PDF( wo, wi_ret );
return EvaluateScatteredEnergy( wo, wi_ret );
}
/* shoot on circle */
uint8_t dc_circle(float interval, float start)
{
dc_autoshoot = DC_AUTOSHOOT_CIRCLE;
dc_gps_count = 0;
dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.);
if (start == DC_IGNORE) {
start = DegOfRad(stateGetNedToBodyEulers_f()->psi);
}
dc_circle_start_angle = fmodf(start, 360.);
if (start < 0.) {
start += 360.;
}
//dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval);
dc_circle_last_block = 0;
dc_circle_max_blocks = floorf(360. / dc_circle_interval);
dc_info();
return 0;
}
int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
{
uint8_t x_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_X_VARIANCE;
uint8_t y_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Y_VARIANCE;
uint8_t z_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Z_VARIANCE;
if (PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[x_variance])) {
// check if the mocap data is valid based on the covariances
_mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[x_variance],
_sub_mocap_odom.get().pose_covariance[y_variance]));
_mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[z_variance]);
_mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV;
_mocap_z_valid = _mocap_epv <= EP_MAX_STD_DEV;
} else {
// if we don't have covariances, assume every reading
_mocap_xy_valid = true;
_mocap_z_valid = true;
}
if (!_mocap_xy_valid || !_mocap_z_valid) {
_time_last_mocap = _sub_mocap_odom.get().timestamp;
return -1;
} else {
_time_last_mocap = _sub_mocap_odom.get().timestamp;
if (PX4_ISFINITE(_sub_mocap_odom.get().x)) {
y.setZero();
y(Y_mocap_x) = _sub_mocap_odom.get().x;
y(Y_mocap_y) = _sub_mocap_odom.get().y;
y(Y_mocap_z) = _sub_mocap_odom.get().z;
_mocapStats.update(y);
return OK;
} else {
return -1;
}
}
}
static void print_bounds(zarray_t * vertices)
{
// Approximate centroid: Just average all vertices, regardless of
// the frequency they are referenced by an index
float minMax[3][2] = {{FLT_MAX,-FLT_MAX},{FLT_MAX,-FLT_MAX},{FLT_MAX,-FLT_MAX}};
for (int i = 0; i < zarray_size(vertices); i++) {
float pt[3];
zarray_get(vertices, i, &pt);
// iterate over xyz coords
for (int j = 0; j < 3; j++) {
minMax[j][0] = fminf(pt[j], minMax[j][0]);
minMax[j][1] = fmaxf(pt[j], minMax[j][1]);
}
}
printf(" Object bounds: xyz [%.2f, %.2f][%.2f, %.2f][%.2f, %.2f]\n",
minMax[0][0],minMax[0][1],
minMax[1][0],minMax[1][1],
minMax[2][0],minMax[2][1]);
}
// on "init" you need to initialize your instance
bool HelloWorld::init()
{
//////////////////////////////
// 1. super init first
if ( !Layer::init() )
{
return false;
}
{
auto background = ({
Sprite *sprite = Sprite::create("home_babies.png");
Size winSize = Director::getInstance()->getWinSize();
sprite->setPosition(Vec2(winSize.width/2, winSize.height/2));
float scaleX = winSize.width / sprite->getContentSize().width;
float scaleY = winSize.height / sprite->getContentSize().height;
float scale = fmaxf(scaleY, scaleX);
sprite->setScale(scale);
sprite;
});
this->addChild(background, 0);
}
void Renderer::calcSceneParams(unsigned int w, unsigned int h,
float* offsets) {
// number of cells along the larger screen dimension
const float NCELLS_MAJOR = MAX_INSTANCES_PER_SIDE;
// cell size in scene space
const float CELL_SIZE = 2.0f / NCELLS_MAJOR;
// Calculations are done in "landscape", i.e. assuming dim[0] >= dim[1].
// Only at the end are values put in the opposite order if h > w.
const float dim[2] = {fmaxf(w,h), fminf(w,h)};
const float aspect[2] = {dim[0] / dim[1], dim[1] / dim[0]};
const float scene2clip[2] = {1.0f, aspect[0]};
const long ncells[2] = {
(long)NCELLS_MAJOR,
(long)floorf(NCELLS_MAJOR * aspect[1])
};
float centers[2][MAX_INSTANCES_PER_SIDE];
for (int d = 0; d < 2; d++) {
float offset = -ncells[d] / NCELLS_MAJOR; // -1.0 for d=0
for (int i = 0; i < ncells[d]; i++) {
centers[d][i] = scene2clip[d] * (CELL_SIZE*(i + 0.5f) + offset);
}
}
int major = w >= h ? 0 : 1;
int minor = w >= h ? 1 : 0;
// outer product of centers[0] and centers[1]
for (int i = 0; i < ncells[0]; i++) {
for (int j = 0; j < ncells[1]; j++) {
int idx = i*ncells[1] + j;
offsets[2*idx + major] = centers[0][i];
offsets[2*idx + minor] = centers[1][j];
}
}
mNumInstances = ncells[0] * ncells[1];
mScale[major] = 0.5f * CELL_SIZE * scene2clip[0];
mScale[minor] = 0.5f * CELL_SIZE * scene2clip[1];
}
// calculate filtered offset between GPS height measurement and EKF height estimate
// offset should be subtracted from GPS measurement to match filter estimate
// offset is used to switch reversion to GPS from alternate height data source
void NavEKF3_core::calcFiltGpsHgtOffset()
{
// Estimate the WGS-84 height of the EKF's origin using a Bayes filter
// calculate the variance of our a-priori estimate of the ekf origin height
float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f);
if (activeHgtSource == HGT_SOURCE_BARO) {
// Use the baro drift rate
const float baroDriftRate = 0.05f;
ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
} else if (activeHgtSource == HGT_SOURCE_RNG) {
// use the worse case expected terrain gradient and vehicle horizontal speed
const float maxTerrGrad = 0.25f;
ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime);
} else if (activeHgtSource == HGT_SOURCE_GPS) {
// by definition we are using GPS height as the EKF datum in this mode
// so cannot run this filter
return;
}
lastOriginHgtTime_ms = imuDataDelayed.time_ms;
// calculate the observation variance assuming EKF error relative to datum is independant of GPS observation error
// when not using GPS as height source
float originHgtObsVar = sq(gpsHgtAccuracy) + P[8][8];
// calculate the correction gain
float gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);
// calculate the innovation
float innovation = - stateStruct.position.z - gpsDataDelayed.hgt;
// check the innovation variance ratio
float ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);
// correct the EKF origin and variance estimate if the innovation variance ratio is < 5-sigma
if (ratio < 5.0f) {
EKF_origin.alt -= (int)(100.0f * gain * innovation);
ekfOriginHgtVar -= fmaxf(gain * ekfOriginHgtVar , 0.0f);
}
}
static gboolean
expose (GtkWidget *widget, GdkEventExpose *event, dt_iop_module_t *self)
{
// capture gui color picked event.
if(darktable.gui->reset) return FALSE;
if(self->picked_color_max[0] < self->picked_color_min[0]) return FALSE;
if(!self->request_color_pick) return FALSE;
static float old[3] = {0, 0, 0};
const float *grayrgb = self->picked_color;
if(grayrgb[0] == old[0] && grayrgb[1] == old[1] && grayrgb[2] == old[2]) return FALSE;
for(int k=0; k<3; k++) old[k] = grayrgb[k];
dt_iop_temperature_params_t *p = (dt_iop_temperature_params_t *)self->params;
for(int k=0; k<3; k++) p->coeffs[k] = (grayrgb[k] > 0.001f) ? 1.0f/grayrgb[k] : 1.0f;
// normalize green:
p->coeffs[0] /= p->coeffs[1];
p->coeffs[2] /= p->coeffs[1];
p->coeffs[1] = 1.0;
for(int k=0; k<3; k++) p->coeffs[k] = fmaxf(0.0f, fminf(8.0f, p->coeffs[k]));
gui_update_from_coeffs(self);
dt_dev_add_history_item(darktable.develop, self, TRUE);
return FALSE;
}
static inline void process_filmic(struct dt_iop_module_t *self, dt_dev_pixelpipe_iop_t *piece,
void *ivoid, void *ovoid, const dt_iop_roi_t *roi_in, const dt_iop_roi_t *roi_out,
dt_iop_global_tonemap_data_t *data)
{
float *in = (float *)ivoid;
float *out = (float *)ovoid;
const int ch = piece->colors;
#ifdef _OPENMP
#pragma omp parallel for default(none) shared(roi_out, in, out, data) schedule(static)
#endif
for(size_t k=0; k<(size_t)roi_out->width*roi_out->height; k++)
{
float *inp = in + ch*k;
float *outp = out + ch*k;
float l = inp[0]/100.0;
float x = fmaxf(0.0f, l-0.004f);
outp[0] = 100.0 * ((x*(6.2*x+.5))/(x*(6.2*x+1.7)+0.06));
outp[1] = inp[1];
outp[2] = inp[2];
}
}
css_dim_t measure(void *context, float width, float height) {
const char *text = (const char *)context;
css_dim_t dim;
if (strcmp(text, SMALL_TEXT) == 0) {
if (width != width) {
width = 1000000;
}
dim.dimensions[CSS_WIDTH] = fminf(SMALL_WIDTH, width);
dim.dimensions[CSS_HEIGHT] = SMALL_HEIGHT;
return dim;
}
if (strcmp(text, LONG_TEXT) == 0) {
if (width != width) {
width = 1000000;
}
dim.dimensions[CSS_WIDTH] = width >= BIG_WIDTH ? BIG_WIDTH : fmaxf(BIG_MIN_WIDTH, width);
dim.dimensions[CSS_HEIGHT] = width >= BIG_WIDTH ? SMALL_HEIGHT : BIG_HEIGHT;
return dim;
}
if (strcmp(text, MEASURE_WITH_RATIO_2) == 0) {
if (width > 0) {
dim.dimensions[CSS_WIDTH] = width;
dim.dimensions[CSS_HEIGHT] = width * 2;
} else if (height > 0) {
dim.dimensions[CSS_WIDTH] = height * 2;
dim.dimensions[CSS_HEIGHT] = height;
} else {
dim.dimensions[CSS_WIDTH] = 99999;
dim.dimensions[CSS_HEIGHT] = 99999;
}
return dim;
}
// Should not go here
dim.dimensions[CSS_WIDTH] = CSS_UNDEFINED;
dim.dimensions[CSS_HEIGHT] = CSS_UNDEFINED;
return dim;
}
css_dim_t measure(void *context, float width) {
const char *text = context;
css_dim_t dim;
if (width != width) {
width = 1000000;
}
if (strcmp(text, SMALL_TEXT) == 0) {
dim.dimensions[CSS_WIDTH] = fminf(SMALL_WIDTH, width);
dim.dimensions[CSS_HEIGHT] = SMALL_HEIGHT;
return dim;
}
if (strcmp(text, LONG_TEXT) == 0) {
dim.dimensions[CSS_WIDTH] = width >= BIG_WIDTH ? BIG_WIDTH : fmaxf(BIG_MIN_WIDTH, width);
dim.dimensions[CSS_HEIGHT] = width >= BIG_WIDTH ? SMALL_HEIGHT : BIG_HEIGHT;
return dim;
}
// Should not go here
dim.dimensions[CSS_WIDTH] = CSS_UNDEFINED;
dim.dimensions[CSS_HEIGHT] = CSS_UNDEFINED;
return dim;
}
bool sphere_intersect(sphere_t s, vector_t ray_start, vector_t ray_direction, vector_t* hit)
{
float a = vector_magnitude_2(ray_direction);
vector_t recentered = vector_sub(ray_start, s.center);
float b = 2 * vector_dot(ray_direction, recentered);
float c = vector_magnitude_2(recentered) - (s.radius * s.radius);
float discrim = (b * b) - (4.0 * a * c);
if (discrim < 0.0)
return false;
float sqrt_discrim = sqrtf(discrim);
float t1 = (-b + sqrt_discrim) / (2.0 * a);
float t2 = (-b - sqrt_discrim) / (2.0 * a);
float t;
if (t1 > 0.0 && t2 > 0.0) {
t = fminf(t1, t2);
} else {
t = fmaxf(t1, t2);
}
if (t <= 0.0)
return false;
*hit = vector_add(ray_start, vector_scale(ray_direction, t));
return true;
}
void solution_check(central2d_t* sim)
{
int nx = sim->nx, ny = sim->ny;
float* u = sim->u;
float h_sum = 0, hu_sum = 0, hv_sum = 0;
float hmin = u[central2d_offset(sim,0,0,0)];
float hmax = hmin;
for (int j = 0; j < ny; ++j)
for (int i = 0; i < nx; ++i) {
float h = u[central2d_offset(sim,0,i,j)];
h_sum += h;
hu_sum += u[central2d_offset(sim,1,i,j)];
hv_sum += u[central2d_offset(sim,2,i,j)];
hmax = fmaxf(h, hmax);
hmin = fminf(h, hmin);
}
float cell_area = sim->dx * sim->dy;
h_sum *= cell_area;
hu_sum *= cell_area;
hv_sum *= cell_area;
printf("-\n Volume: %g\n Momentum: (%g, %g)\n Range: [%g, %g]\n",
h_sum, hu_sum, hv_sum, hmin, hmax);
assert(hmin > 0);
}
static void testSize()
{
start_test("Lock free FIFO - size");
const int es = sizeof(int);
const int c = 100;
const int nCases = 5;
int nPush[nCases] = {10, 20, 30, 5, 90};
int nPop[nCases] = {5, 1, 60, 10, 90};
for (int i = 0; i < nCases; i++)
{
drLockFreeFIFO f;
drLockFreeFIFO_init(&f, c, es);
for (int j = 0; j < nPush[i]; j++)
{
int success = drLockFreeFIFO_push(&f, &j);
}
for (int j = 0; j < nPop[i]; j++)
{
int val = 0;
int success = drLockFreeFIFO_pop(&f, &val);
}
const int expectedSize = fmaxf(0.0f, nPush[i] - nPop[i]);
const int size = drLockFreeFIFO_getNumElements(&f);
fail_unless(expectedSize == size, "FIFO size should be the same after pushing and popping the same number of items");
drLockFreeFIFO_deinit(&f);
}
}
// untested - algorithm sound but could be typos
CollisionData sat_hull_ray(const ConvexHull &A, const ray &r)
{
CollisionData cd = { false, INFINITY }; // setup return value
std::vector<vec2> axes;
axes.reserve(A.verts.size());
for (int i = 0; i < A.verts.size(); ++i)
axes.push_back(perp(normal(A.verts[i] - A.verts[(i + 1) % A.verts.size()])));
float tmin = 0, //"Entering" scalar for the ray
tmax = 1; //"Leaving" scalar for the ray
vec2 cnormal;
float tpmin;
for (int i = 0; i < axes.size(); ++i)
{
float N = dot(axes[i], r.position - A.verts[i]);
float D = -dot(axes[i], r.direction);
float t = N / D;
if (D < 0 && t > tmin)
{
tmin = fmaxf(tmin, t);
cnormal = axes[i];
cd = { tmin < tmax, (tmax-tmin) * r.length, axes[i], r.position + r.direction * r.length *tmin };
}
else tmax = fminf(tmax, t);
if (tmin > tmax) return cd;
}
return cd;
}
/// clamp each vector values
inline __host__ __device__ Vec3 clamp(const Vec3& min_v, const Vec3& max_v) const {
return Vec3( fminf( fmaxf(x, min_v.x), max_v.x),
fminf( fmaxf(y, min_v.y), max_v.y),
fminf( fmaxf(z, min_v.z), max_v.z));
}
/// clamp each vector values
inline __host__ __device__ Vec3 clamp(float min_v, float max_v) const {
return Vec3( fminf( fmaxf(x, min_v), max_v),
fminf( fmaxf(y, min_v), max_v),
fminf( fmaxf(z, min_v), max_v));
}
/// value of the max coordinate
inline __host__ __device__ float get_max() const {
return fmaxf(fmaxf(x,y),z);
}
/////////////////////////////////////////////////////////////////////////////////
// calculate the intersection of a sphere the given ray
// the ray has an origin and a direction, ray = origin + t*direction
// find the t parameter, return true if it is between 0.0 and 1.0, false
// otherwise, write the results in following variables:
// depth - t \in [0.0 1.0]
// posX - x position of intersection point, nothing if no intersection
// posY - y position of intersection point, nothing if no intersection
// posZ - z position of intersection point, nothing if no intersection
// normalX - x component of normal at intersection point, nothing if no intersection
// normalX - y component of normal at intersection point, nothing if no intersection
// normalX - z component of normal at intersection point, nothing if no intersection
//
// attention: a sphere has usually two intersection points make sure to return
// the one that is closest to the ray's origin and still in the viewing frustum
//
/////////////////////////////////////////////////////////////////////////////////
bool
Sphere::intersect(Ray ray, double *depth,
double *posX, double *posY, double *posZ,
double *normalX, double *normalY, double *normalZ)
{
//////////*********** START OF CODE TO CHANGE *******////////////
// from slides:
// (cx + t * vx)^2 + (cy + t * vy)^2 + (cz + t * vy)^2 = r^2
// text:
// (e+td−c)·(e+td−c)−R2 = 0
// (d·d)t^2 +2d·(e−c)t+(e−c)·(e−c)−R^2 = 0
// d: the direction vector of the ray
// e: point at which the ray starts
// c: center point of the sphere
Vec3 dvec( ray.direction[0],
ray.direction[1],
ray.direction[2]);
Vec3 evec( ray.origin[0],
ray.origin[1],
ray.origin[2]);
Vec3 cvec( this->center[0],
this->center[1],
this->center[2]);
// use the quadratic equation, since we have the form At^2 + Bt + C = 0.
double a = dvec.dot(dvec);
double b = dvec.scale(2).dot(evec.subtract(cvec));
Vec3 eMinusCvec = evec.subtract(cvec);
double c = eMinusCvec.dot(eMinusCvec) - (this->radius * this->radius);
// discriminant: b^2 - 4ac
double discriminant = (b * b) - (4 * a * c);
// From text: If the discriminant is negative, its square root
// is imaginary and the line and sphere do not intersect.
if (discriminant < 0) {
//printf("No intersection with sphere - 1\n");
return false;
} else {
// there is at least one intersection point
double t1 = (-b + sqrt(discriminant)) / (2 * a);
double t2 = (-b - sqrt(discriminant)) / (2 * a);
double tmin = fminf(t1, t2);
double tmax = fmaxf(t1, t2);
double t = 0; // t is set to either tmin or tmax (or the function returns false)
if (tmin >= 0) { //} && tmin <= 1) {
t = tmin;
} else if (tmax >= 0) { //} && tmax <= 1) {
t = tmax;
} else {
// return false if neither interestion point is within [0, 1]
//printf("No intersection with sphere. t values (%f, %f)\n", t1, t2);
return false;
}
*depth = t;
// position: (e + td)
Vec3 posvec = dvec.scale(t).add(evec);
*posX = posvec[0];
*posY = posvec[1];
*posZ = posvec[2];
// normal: 2(p - c)
Vec3 normalvec = posvec.subtract(cvec).scale(2);
normalvec.normalize();
*normalX = normalvec[0];
*normalY = normalvec[1];
*normalZ = normalvec[2];
}
//////////*********** END OF CODE TO CHANGE *******////////////
//printf("Sphere intersection found (%f, %f, %f) \n", *posX, *posY, *posZ);
return true;
}
float ArmClass::Validate_Lift_Command(float cmd, bool ispidcmd)
{
#if 01
// Validating the lift command.
// We broke the limits down into two cases.
// 1) When the turret is facing forward (0deg) the arm can go down to 0deg or up to (180-39.2 = 140.8deg)
// 2) When the turret is facing sideways (90deg or 270deg) the arm can go down to 14.6deg or up to (180-14.6 = 165.4deg)
int lift_enc = GetLifterEncoder();
float lift_angle = Lift_Encoder_To_Degrees(lift_enc);
int turret_enc = GetTurretEncoder();
float turret_angle = Turret_Encoder_To_Degrees(turret_enc);
// Special case: Turret is centered.
if (TurretRoughlyCentered())
{
if (cmd >= 0.0f) // if we are pushing the arm down (might have to switch this to > 0.0
{
if (lift_angle < ARM_LIFT_MIN_WHEN_CENTERED)
{
// BAD! pushing the arm into the frame of the robot
float error = ARM_LIFT_MIN_WHEN_CENTERED - lift_angle;
return Compute_Lift_Error_Correction_Command(error);
}
}
if (cmd <= 0.0f) // lifting, limit at 140 deg
{
if (lift_angle > ARM_LIFT_MAX_WHEN_CENTERED)
{
float error = ARM_LIFT_MAX_WHEN_CENTERED - lift_angle;
return Compute_Lift_Error_Correction_Command(error);
}
}
}
else if (fabs(turret_angle) < 45.0f) // Mostly Forward case
{
if (cmd >= 0.0f) // if we are pushing the arm down (might have to switch this to > 0.0
{
if (lift_angle < ARM_LIFT_MIN_WHEN_FWD)
{
// BAD! pushing the arm into the frame of the robot
float error = ARM_LIFT_MIN_WHEN_FWD - lift_angle;
return Compute_Lift_Error_Correction_Command(error);
}
}
if (cmd <= 0.0f) // lifting, limit at 140 deg
{
if (lift_angle > ARM_LIFT_MAX_WHEN_FWD)
{
float error = ARM_LIFT_MAX_WHEN_FWD - lift_angle;
return Compute_Lift_Error_Correction_Command(error);
}
}
}
else // Sideways case
{
if (cmd >= 0.0f) // if we are pushing the arm down (might have to switch this to > 0.0
{
if (lift_angle < ARM_LIFT_MIN_WHEN_SIDEWAYS)
{
// BAD! violating the envelop to one side of the robot
float error = ARM_LIFT_MIN_WHEN_SIDEWAYS - lift_angle;
return Compute_Lift_Error_Correction_Command(error);
}
}
if (cmd <= 0.0f) // lifting, limit at 140 deg
{
if (lift_angle > ARM_LIFT_MAX_WHEN_SIDEWAYS)
{
// BAD! violating the envelope to the other side of the robot
float error = ARM_LIFT_MAX_WHEN_SIDEWAYS - lift_angle;
return Compute_Lift_Error_Correction_Command(error);
}
}
}
#endif
if(ispidcmd)
{
//randon .002 commands...
if(cmd > 0.003f)
{
cmd = fmaxf(cmd, MIN_ARM_LIFT_CMD);
}
else if (cmd < -.003f)
{
cmd = fminf(cmd, -MIN_ARM_LIFT_CMD);
}
else if (cmd < .003f && cmd > -.003f)
{
cmd = 0.0f;
}
}
return cmd;
}
int main(int argc, char ** argv) {
/* BEGIN DECLARATIONS */
WINFO wi; /* struct for command line input */
/* workspace */
float * v; /* velocity field */
float * p1; /* pressure field, current time step */
float * p0; /* pressure field, last time step */
float * tr; /* storage for traces */
float * tmp; /* used to swap p1 and p0 */
int ix, it; /* counters */
int isrc; /* source counter */
int imf; /* movie frame counter */
int isx; /* source location, in units of dx */
int nxz; /* number of spatial grid points */
/* int nz; local number of gridpoints */
int ntr; /* number of traces */
int nsam; /* number of trace samples */
int nsrc; /* number of shots */
float rz,rx,s; /* precomputed coefficients */
float vmax,vmin; /* max, min velocity values */
/* float two; two */
/* END DECLARATIONS */
sf_init(argc,argv);
/* read inputs from command line */
getinputs(true,&wi);
/* compute number of shots */
nsrc = (wi.isxend-wi.isxbeg)/(wi.iskip); nsrc++;
/* compute number of spatial grid points */
nxz=wi.nx * wi.nz;
/* compute number of traces, samples in each record */
ntr=wi.igxend-wi.igxbeg+1;
nsam=ntr*wi.nt;
/* allocate, initialize p0, p1, v, traces */
p0=sf_floatalloc(nxz);
p1=sf_floatalloc(nxz);
v =sf_floatalloc(nxz);
tr=sf_floatalloc(nsam);
/* read velocity */
sf_floatread(v,nxz,wi.vfile);
/* CFL, sanity checks */
vmax=fgetmax(v,nxz);
vmin=fgetmin(v,nxz);
if (vmax*wi.dt>CFL*fmaxf(wi.dx,wi.dz)) {
sf_warning("CFL criterion violated");
sf_warning("vmax=%e dx=%e dz=%e dt=%e\n",vmax,wi.dx,wi.dz,wi.dt);
sf_error("max permitted dt=%e\n",CFL*fmaxf(wi.dx,wi.dz)/vmax);
}
if (vmin<=0.0)
sf_error("min velocity nonpositive");
/* only square of velocity array needed from here on */
fsquare(v,nxz);
/* precalculate some coefficients */
rz=wi.dt*wi.dt/(wi.dz*wi.dz);
rx=wi.dt*wi.dt/(wi.dx*wi.dx);
s =2.0*(rz+rx);
/* two=2.0;
nz=wi.nz; */
/* shot loop */
isrc=0;
isx=wi.isxbeg;
while (isx <= wi.isxend) {
/* initialize pressure fields, traces */
fzeros(p0,nxz);
fzeros(p1,nxz);
fzeros(tr,nsam);
/* initialize movie frame counter */
imf=0;
/* time loop */
for (it=0;it<wi.nt;it++) {
/* construct next time step, overwrite on p0 */
step_forward(p0,p1,v,wi.nz,wi.nx,rz,rx,s);
/* tack on source */
p0[wi.isz+isx*wi.nz]+=fgetrick(it*wi.dt,wi.freq);
/* swap pointers */
tmp=p0;
p0=p1;
p1=tmp;
/* store trace samples if necessary */
if (NULL != wi.tfile)
for (ix=0;ix<ntr;ix++)
tr[ix*wi.nt+it]=p1[(wi.igxbeg+ix)*wi.nz+wi.igz];
/* write movie snap to file if necessary */
if (NULL != wi.mfile && wi.nm && !(it%wi.nm)) {
sf_floatwrite(p1,nxz,wi.mfile);
imf++;
}
/* next t */
}
/* write traces to file if necessary */
if (NULL != wi.tfile)
sf_floatwrite(tr,nsam,wi.tfile);
isx += wi.iskip;
isrc++;
}
exit(0);
}
void BattleGesture::UpdateTrackingMarker()
{
Unit* unit = _trackingMarker->GetUnit();
glm::vec2 screenTouchPosition = _trackingTouch->GetPosition();
glm::vec2 screenMarkerPosition = screenTouchPosition + glm::vec2(0, 1) * (_offsetToMarker * GetFlipSign());
glm::vec2 touchPosition = _battleView->GetTerrainPosition3(screenTouchPosition).xy();
glm::vec2 markerPosition = _battleView->GetTerrainPosition3(screenMarkerPosition).xy();
Unit* enemyUnit = FindEnemyUnit(touchPosition, markerPosition);
glm::vec2 unitCenter = unit->state.center;
bool isModifierMode = _tappedModiferArea || _modifierTouch != nullptr || _trackingTouch->GetCurrentButtons().right;
_trackingMarker->SetRenderOrientation(isModifierMode);
if (!isModifierMode)
{
std::vector<glm::vec2>& path = _trackingMarker->_path;
glm::vec2 currentDestination = path.size() != 0 ? *(path.end() - 1) : unit->state.center;
bounds2f contentBounds = _battleView->GetContentBounds();
glm::vec2 contentCenter = contentBounds.center();
float contentRadius = contentBounds.width() / 2;
glm::vec2 differenceToCenter = contentCenter - markerPosition;
float distanceToCenter = glm::length(differenceToCenter);
if (distanceToCenter > contentRadius)
{
markerPosition += differenceToCenter * (distanceToCenter - contentRadius) / distanceToCenter;
}
float movementLimit = -1;
float delta = 1.0f / fmaxf(1, glm::length(currentDestination - markerPosition));
for (float k = delta; k < 1; k += delta)
{
GroundMap* groundMap = _battleView->GetSimulator()->GetGroundMap();
if (groundMap != nullptr && groundMap->IsImpassable(glm::mix(currentDestination, markerPosition, k)))
{
movementLimit = k;
break;
}
}
if (movementLimit >= 0)
{
glm::vec2 diff = markerPosition - currentDestination;
markerPosition = currentDestination + diff * movementLimit;
markerPosition -= glm::normalize(diff) * 10.0f;
enemyUnit = nullptr;
}
if (enemyUnit && !_trackingMarker->GetMeleeTarget())
SoundPlayer::singleton->Play(SoundBufferCommandMod);
_trackingMarker->SetMeleeTarget(enemyUnit);
_trackingMarker->SetDestination(&markerPosition);
if (enemyUnit != nullptr)
MovementRules::UpdateMovementPath(_trackingMarker->_path, unitCenter, enemyUnit->state.center);
else
MovementRules::UpdateMovementPath(_trackingMarker->_path, unitCenter, markerPosition);
if (enemyUnit != nullptr)
{
glm::vec2 destination = enemyUnit->state.center;
glm::vec2 orientation = destination + glm::normalize(destination - unitCenter) * 18.0f;
_trackingMarker->SetOrientation(&orientation);
}
else if (MovementRules::Length(_trackingMarker->_path) > KEEP_ORIENTATION_TRESHHOLD)
{
glm::vec2 dir = glm::normalize(markerPosition - unitCenter);
if (path.size() >= 2)
dir = glm::normalize(*(path.end() - 1) - *(path.end() - 2));
glm::vec2 orientation = markerPosition + dir * 18.0f;
_trackingMarker->SetOrientation(&orientation);
}
else
{
glm::vec2 orientation = markerPosition + 18.0f * vector2_from_angle(unit->state.bearing);
_trackingMarker->SetOrientation(&orientation);
}
}
else
{
MovementRules::UpdateMovementPathStart(_trackingMarker->_path, unitCenter);
bool holdFire = false;
if (_trackingMarker->GetUnit()->state.unitMode == UnitMode_Standing && _trackingMarker->GetUnit()->stats.maximumRange > 0)
{
bounds2f unitCurrentBounds = GetUnitCurrentBounds(_trackingMarker->GetUnit());
holdFire = glm::distance(screenMarkerPosition, unitCurrentBounds.center()) <= unitCurrentBounds.x().radius();
}
if (holdFire)
{
_trackingMarker->SetMissileTarget(_trackingMarker->GetUnit());
_trackingMarker->SetOrientation(nullptr);
}
else
{
//if (!_tappedUnitCenter)
// enemyUnit = nullptr;
if (!_allowTargetEnemyUnit)
enemyUnit = nullptr;
if (enemyUnit != nullptr && _trackingMarker->GetMissileTarget() == nullptr)
SoundPlayer::singleton->Play(SoundBufferCommandMod);
_trackingMarker->SetMissileTarget(enemyUnit);
_trackingMarker->SetOrientation(&markerPosition);
}
}
}
void
Mission::set_mission_items()
{
/* make sure param is up to date */
updateParams();
/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
altitude_sp_foh_reset();
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* set previous position setpoint to current */
set_previous_pos_setpoint();
/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
if (pos_sp_triplet->previous.valid) {
_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
}
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = false;
/* try setting onboard mission item */
if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_ONBOARD;
/* try setting offboard mission item */
} else if (read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_OFFBOARD;
} else {
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
user_feedback_done = true;
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
}
_mission_type = MISSION_TYPE_NONE;
/* set loiter mission item and ensure that there is a minimum clearance from home */
set_loiter_item(&_mission_item, _param_takeoff_alt.get());
/* update position setpoint triplet */
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
/* reuse setpoint for LOITER only if it's not IDLE */
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
set_mission_finished();
if (!user_feedback_done) {
/* only tell users that we got no mission if there has not been any
* better, more specific feedback yet
* https://en.wikipedia.org/wiki/Loiter_(aeronautics)
*/
if (_navigator->get_vstatus()->condition_landed) {
/* landed, refusing to take off without a mission */
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
} else {
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
}
user_feedback_done = true;
}
_navigator->set_position_setpoint_triplet_updated();
return;
}
if (pos_sp_triplet->current.valid) {
_on_arrival_yaw = _mission_item.yaw;
}
/* do takeoff on first waypoint for rotary wing vehicles */
if (_navigator->get_vstatus()->is_rotary_wing) {
/* force takeoff if landed (additional protection) */
if (!_takeoff && _navigator->get_vstatus()->condition_landed) {
_need_takeoff = true;
}
/* new current mission item set, check if we need takeoff */
if (_need_takeoff && (
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
_takeoff = true;
_need_takeoff = false;
}
}
if (_takeoff) {
/* do takeoff before going to setpoint */
/* set mission item as next position setpoint */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
/* next SP is not takeoff anymore */
pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
/* calculate takeoff altitude */
float takeoff_alt = get_absolute_altitude_for_item(_mission_item);
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_vstatus()->condition_landed) {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
} else {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
/* check if we already above takeoff altitude */
if (_navigator->get_global_position()->alt < takeoff_alt) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.yaw = NAN;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
return;
} else {
/* skip takeoff */
_takeoff = false;
}
}
if (_takeoff_finished) {
/* we just finished takeoff */
/* in case we still have to move to the takeoff waypoint we need a waypoint mission item */
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_takeoff_finished = false;
}
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
if (_mission_type == MISSION_TYPE_OFFBOARD) {
set_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
/* try to read next mission item */
struct mission_item_s mission_item_next;
if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
} else {
/* next mission item is not available */
pos_sp_triplet->next.valid = false;
}
} else {
/* vehicle will be paused on current waypoint, don't set next item */
pos_sp_triplet->next.valid = false;
}
/* Save the distance between the current sp and the previous one */
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
_distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon,
pos_sp_triplet->previous.lat,
pos_sp_triplet->previous.lon);
}
_navigator->set_position_setpoint_triplet_updated();
}
