/// \brief Keep the value of \p infinity as small as possible to improve precision in Winding_Clip.
void Winding_createInfinite(FixedWinding& winding, const Plane3& plane, double infinity)
{
double max = -infinity;
int x = -1;
for (int i=0 ; i<3; i++)
{
double d = fabs(plane.normal()[i]);
if (d > max)
{
x = i;
max = d;
}
}
if(x == -1)
{
globalErrorStream() << "invalid plane\n";
return;
}
DoubleVector3 vup = g_vector3_identity;
switch (x)
{
case 0:
case 1:
vup[2] = 1;
break;
case 2:
vup[0] = 1;
break;
}
vector3_add(vup, vector3_scaled(plane.normal(), -vector3_dot(vup, plane.normal())));
vector3_normalise(vup);
DoubleVector3 org = vector3_scaled(plane.normal(), plane.dist());
DoubleVector3 vright = vector3_cross(vup, plane.normal());
vector3_scale(vup, infinity);
vector3_scale(vright, infinity);
// project a really big axis aligned box onto the plane
DoubleLine r1, r2, r3, r4;
r1.origin = vector3_added(vector3_subtracted(org, vright), vup);
r1.direction = vector3_normalised(vright);
winding.push_back(FixedWindingVertex(r1.origin, r1, c_brush_maxFaces));
r2.origin = vector3_added(vector3_added(org, vright), vup);
r2.direction = vector3_normalised(vector3_negated(vup));
winding.push_back(FixedWindingVertex(r2.origin, r2, c_brush_maxFaces));
r3.origin = vector3_subtracted(vector3_added(org, vright), vup);
r3.direction = vector3_normalised(vector3_negated(vright));
winding.push_back(FixedWindingVertex(r3.origin, r3, c_brush_maxFaces));
r4.origin = vector3_subtracted(vector3_subtracted(org, vright), vup);
r4.direction = vector3_normalised(vup);
winding.push_back(FixedWindingVertex(r4.origin, r4, c_brush_maxFaces));
}
/**
* @brief Read magnetometer vector (3-axis) data
*
* This function obtains magnetometer data for all three axes of the Honeywell
* device. The data is read from six device registers using a multi-byte
* bus transfer. The 10-bit raw results are then assembled from the two
* register values for each axis, including extending the sign bit, to
* form a signed 32-bit value.
*
* Along with the actual sensor data, the LSB byte contains a "new" flag
* indicating if the data for this axis has been updated since the last
* time the axis data was read. Reading either LSB or MSB data will
* clear this flag.
*
* @param hal Address of an initialized sensor device descriptor.
* @param data The address of a vector storing sensor axis data.
* @return bool true if the call succeeds, else false is returned.
*/
static bool hmc5883l_get_field(sensor_hal_t *hal, sensor_data_t *data)
{
/* Get magnetic field measurements & test for data overflow. */
vector3_t mag_data;
bool result = hmc5883l_get_data(hal, &mag_data);
if (result) {
if (data->scaled) {
/* Apply sensitivity adjustment to data */
hmc5883l_apply_sensitivity(&mag_data);
/* Apply measurement offsets to data */
hmc5883l_apply_offset(&mag_data);
/* Scale output values to SI units (uTesla) */
scalar_t const scale = (scalar_t)GAUSS_TO_MICRO_TESLA /
scale_table[dev_range];
vector3_scale(scale, &mag_data);
}
data->axis.x = (int32_t)mag_data.x;
data->axis.y = (int32_t)mag_data.y;
data->axis.z = (int32_t)mag_data.z;
}
return result;
}
/*! \brief Calculate direction, inclination, and field strength
*
* This routine calculates horizontal direction, vertical inclination and
* net field magnitude for a "raw" (unscaled) magnetic \c field sample from
* a 3-axis magnetometer.
*
* \param field Magnetometer raw vector field sample input
* \param theta Calculated direction angle (degrees) output
* \param delta Calculated inclination angle (degrees) output
* \param strength Calculated raw vector field magnitude output
*
* \return bool true if the call succeeds, else false is returned.
*/
bool field_direction(vector3_t *field, scalar_t *theta, scalar_t *delta,
scalar_t *strength)
{
/* Calculate the geomagnetic field direction vector (unit vector) and
* strength (field magnitude).
*/
scalar_t const magnitude = vector3_magnitude(field);
if (0 == magnitude) {
return false;
}
vector3_scale((1 / magnitude), field);
*strength = magnitude;
/* Calculate the direction angle (degrees) assuming no mounting tilt.
* "Wraparound" negative results to a positive heading. The angle is
* calculated relative to +Y axis, so atan2() arguments are (x,y)
* instead of (y,x). Result is inverted (* -1) so positive values
* reflect clockwise rotation.
*/
*theta = degrees(-1 * atan2(field->x, field->y));
if (*theta < 0) {
*theta += 360;
}
/* Calculate the inclination angle degrees (assuming no mounting tilt).
* Downward angle is indicated by a positive inclination value.
*/
*delta = degrees(-1 * asin(field->z));
return true;
}
/**
* @brief Read magnetometer vector (3-axis) data
*
* This function obtains magnetometer data for all three axes of the AKM
* device. The data is read from six device registers using a multi-byte
* bus transfer. The 10-bit raw results are then assembled from the two
* register values for each axis, including extending the sign bit, to
* form a signed 32-bit value.
*
* Along with the actual sensor data, the LSB byte contains a "new" flag
* indicating if the data for this axis has been updated since the last
* time the axis data was read. Reading either LSB or MSB data will
* clear this flag.
*
* @param hal Address of an initialized sensor hardware descriptor.
* @param data The address of a vector storing sensor axis data.
* @return bool true if the call succeeds, else false is returned.
*/
static bool ak8975_get_field(sensor_hal_t *hal, sensor_data_t *data)
{
vector3_t mag_data;
bool result = false;
/* Get magnetic field measurements & test for data overflow. */
if (ak8975_get_data(hal, AK8975_SINGLE_MODE, &mag_data) &&
(!ak8975_check_overflow(&mag_data))) {
if (data->scaled) {
/* Apply stored measurement offsets to data. */
ak8975_apply_offset(&mag_data);
/* Scale output values to SI units (uTesla). */
vector3_scale((scalar_t)MICRO_TESLA_PER_COUNT,
&mag_data);
}
data->axis.x = mag_data.x;
data->axis.y = mag_data.y;
data->axis.z = mag_data.z;
result = true;
}
return result;
}
/* Initialize the dielectric function of the global mdata structure,
along with other geometry data. Should be called from init-params,
or in general when global input vars have been loaded and mdata
allocated. */
void init_epsilon(void)
{
int i;
int tree_depth, tree_nobjects;
number no_size;
no_size = 2.0 / ctl_get_number("infinity");
mpi_one_printf("Mesh size is %d.\n", mesh_size);
no_size_x = geometry_lattice.size.x <= no_size;
no_size_y = geometry_lattice.size.y <= no_size || dimensions < 2;
no_size_z = geometry_lattice.size.z <= no_size || dimensions < 3;
Rm.c0 = vector3_scale(no_size_x ? 1 : geometry_lattice.size.x,
geometry_lattice.basis.c0);
Rm.c1 = vector3_scale(no_size_y ? 1 : geometry_lattice.size.y,
geometry_lattice.basis.c1);
Rm.c2 = vector3_scale(no_size_z ? 1 : geometry_lattice.size.z,
geometry_lattice.basis.c2);
mpi_one_printf("Lattice vectors:\n");
mpi_one_printf(" (%g, %g, %g)\n", Rm.c0.x, Rm.c0.y, Rm.c0.z);
mpi_one_printf(" (%g, %g, %g)\n", Rm.c1.x, Rm.c1.y, Rm.c1.z);
mpi_one_printf(" (%g, %g, %g)\n", Rm.c2.x, Rm.c2.y, Rm.c2.z);
Vol = fabs(matrix3x3_determinant(Rm));
mpi_one_printf("Cell volume = %g\n", Vol);
Gm = matrix3x3_inverse(matrix3x3_transpose(Rm));
mpi_one_printf("Reciprocal lattice vectors (/ 2 pi):\n");
mpi_one_printf(" (%g, %g, %g)\n", Gm.c0.x, Gm.c0.y, Gm.c0.z);
mpi_one_printf(" (%g, %g, %g)\n", Gm.c1.x, Gm.c1.y, Gm.c1.z);
mpi_one_printf(" (%g, %g, %g)\n", Gm.c2.x, Gm.c2.y, Gm.c2.z);
if (eigensolver_nwork > MAX_NWORK) {
mpi_one_printf("(Reducing nwork = %d to maximum: %d.)\n",
eigensolver_nwork, MAX_NWORK);
eigensolver_nwork = MAX_NWORK;
}
matrix3x3_to_arr(R, Rm);
matrix3x3_to_arr(G, Gm);
/* we must do this to correct for a non-orthogonal lattice basis: */
geom_fix_objects();
mpi_one_printf("Geometric objects:\n");
if (mpi_is_master())
for (i = 0; i < geometry.num_items; ++i) {
display_geometric_object_info(5, geometry.items[i]);
if (geometry.items[i].material.which_subclass == MEDIUM)
printf("%*sepsilon = %g, mu = %g\n",
5 + 5, "",
geometry.items[i].material.
subclass.medium_data->epsilon,
geometry.items[i].material.
subclass.medium_data->mu);
}
destroy_geom_box_tree(geometry_tree); /* destroy any tree from
previous runs */
{
geom_box b0;
b0.low = vector3_plus(geometry_center,
vector3_scale(-0.5, geometry_lattice.size));
b0.high = vector3_plus(geometry_center,
vector3_scale(0.5, geometry_lattice.size));
/* pad tree boundaries to allow for sub-pixel averaging */
b0.low.x -= geometry_lattice.size.x / mdata->nx;
b0.low.y -= geometry_lattice.size.y / mdata->ny;
b0.low.z -= geometry_lattice.size.z / mdata->nz;
b0.high.x += geometry_lattice.size.x / mdata->nx;
b0.high.y += geometry_lattice.size.y / mdata->ny;
b0.high.z += geometry_lattice.size.z / mdata->nz;
geometry_tree = create_geom_box_tree0(geometry, b0);
}
if (verbose && mpi_is_master()) {
printf("Geometry object bounding box tree:\n");
display_geom_box_tree(5, geometry_tree);
}
geom_box_tree_stats(geometry_tree, &tree_depth, &tree_nobjects);
mpi_one_printf("Geometric object tree has depth %d and %d object nodes"
" (vs. %d actual objects)\n",
tree_depth, tree_nobjects, geometry.num_items);
reset_epsilon();
}
