/*
INPUT: Data from CAN msg which has been put into the float of the fXYZ structure
OUTPUT: Result stored in file scope variable: MagnetAngularPosition
DESCRIPTION: Calculates the dot product of the incoming measurement
against the reference vector for each of the x=0, y=0, and
z=0 planes.
*/
void magnet_angles( struct fXYZ* mRaw )
{
// Similar to accel_
// Normalize the Raw Magnet vector
// Compared to 1 earth magnet field strength
struct fXYZ ref = magnetReference;
/* Project the baseline vector and the current vector
onto each plane (x=0,y=0,z=0),
mRaw dot_product Ref = cos(angle);
Solve for the angle between them.
*/
// Compare to baseline vector, not the unit vectors!
ref = magnetReference;
ref.x = 0; // yz plane
float S=mRaw->x; mRaw->x=0;
MagnetAngularPosition.rx = rad_to_deg(angle_between( mRaw, &ref ));
mRaw->x=S;
ref = magnetReference;
ref.y = 0; // xz plane
S=mRaw->y; mRaw->y=0;
MagnetAngularPosition.ry = rad_to_deg(angle_between( mRaw, &ref ));
mRaw->y=S;
ref = magnetReference;
ref.z = 0; // xy plane
S=mRaw->z; mRaw->z=0;
MagnetAngularPosition.rz = rad_to_deg(angle_between( mRaw, &ref ));
mRaw->z=S;
// Need to Deal with Quadrants!?
// sensor is responding somewhat correctly. X is length of board.
}
bool RobustViconTracker::poses_are_similar(const SimpleCamera& cam1, const SimpleCamera& cam2, double distanceThreshold, double angleThreshold)
{
// Calculate the distance between the positions of the two cameras, and the angles between their corresponding camera axes.
double distance = (cam1.p() - cam2.p()).norm();
double nAngle = angle_between(cam1.n(), cam2.n());
double uAngle = angle_between(cam1.u(), cam2.u());
double vAngle = angle_between(cam1.v(), cam2.v());
// Check the similarity of the camera poses by comparing the aforementioned distance and angles to suitable thresholds.
return distance < distanceThreshold && nAngle < angleThreshold && uAngle < angleThreshold && vAngle < angleThreshold;
}
void Needle::setTransformFromEndEffectorBoxConstrained(const Vector3d& new_ee_pos, const Matrix3d& new_ee_rot, Box* box)
{
Vector3d old_ee_pos;
Matrix3d old_ee_rot;
getEndEffectorTransform(old_ee_pos, old_ee_rot);
Vector3d old_proj = (old_ee_pos - position);
old_proj.normalize();
Vector3d proj = (new_ee_pos-position).dot(rotation.col(0))*rotation.col(0) + (new_ee_pos-position).dot(rotation.col(2))*rotation.col(2);
proj.normalize();
glColor3f(0.8,0.8,0);
drawArrow(position, 20*proj);
glColor3f(0.8,0,0);
drawArrow(position, 20*old_proj);
Vector3d normal = proj.cross(old_proj);
normal.normalize();
double angle = angle_between(proj, old_proj)*180.0/M_PI;
if (angle > 90.0) {
angle = 0.0;
//TODO return;
} else {
double sign = ((normal - rotation.col(1)).squaredNorm() < 1e-5) ? -1 : 1;
angle = sign*min(angle_between(proj, old_proj)*180.0/M_PI, 5.0);
}
if (!isnan(angle) && abs(angle)>1e-5 && angle>0.0) { //TODO for now, only allows forward movement of needle through box
rotateAboutAxis(angle);
if(isThreadAttached()) {
thread->updateConstrainedTransform(constraint_ind, getEndPosition(), getEndRotation());
}
}
if (isThreadAttached()) {
Vector3d direction;
if (!box->isThreadAttached() && (sphereBoxDistance(getEndPosition(), getThicknessRadius(), box->getPosition(), box->getHalfLength(), direction) < 0)) {
cout << "thread gets into the box" << endl;
//box->attach(getThread());
box->attachThreadIn(getThread(), getEndPosition(), getEndRotation());
}
if (box->isThreadAttached() && !boxCollision(box->getPosition(), box->getHalfLength())) {
cout << "thread gets out of the box" << endl;
box->attachThreadOut(getThread(), getEndPosition(), getEndRotation());
}
}
}
float world_map::turning_angle(edge *leaving, edge *entering, vertex *currently_at)
{
return angle_between(leaving->dirx(currently_at),
leaving->diry(currently_at),
-entering->dirx(currently_at),
-entering->diry(currently_at));
}
float world_map::turning_angle(float dirx, float diry, edge *entering, vertex *currently_at)
{
return angle_between(
dirx,
diry,
-entering->dirx(currently_at),
-entering->diry(currently_at));
}
void v4math_object::test<18>()
{
F32 x = 1.f, y = 2.f, z = -1.1f;
F32 angle1, angle2;
LLVector4 vec4(x,y,z), vec4a(x,y,z);
angle1 = angle_between(vec4, vec4a);
vec4.normVec();
vec4a.normVec();
angle2 = acos(vec4 * vec4a);
ensure_approximately_equals("1:angle_between: Fail " ,angle1,angle2,8);
F32 x1 = 21.f, y1 = 2.23f, z1 = -1.1f;
LLVector4 vec4b(x,y,z), vec4c(x1,y1,z1);
angle1 = angle_between(vec4b, vec4c);
vec4b.normVec();
vec4c.normVec();
angle2 = acos(vec4b * vec4c);
ensure_approximately_equals("2:angle_between: Fail " ,angle1,angle2,8);
}
template<> Array<T> CubicHinges<TV2>::angles(RawArray<const Vector<int,3>> bends, RawArray<const TV2> X) {
if (bends.size())
GEODE_ASSERT(X.size()>scalar_view(bends).max());
Array<T> angles(bends.size(),uninit);
for (int b=0;b<bends.size();b++) {
int i0,i1,i2;bends[b].get(i0,i1,i2);
const TV x0 = X[i0], x1 = X[i1], x2 = X[i2];
angles[b] = angle_between(x1-x0,x2-x1);
}
return angles;
}
void v3dmath_object::test<24>()
{
#if LL_WINDOWS && _MSC_VER < 1400
skip("This fails on VS2003!");
#else
F64 x = 10., y = 20., z = -15.;
F64 angle1, angle2;
LLVector3d vec3Da(x,y,z), vec3Db(x,y,z);
angle1 = angle_between(vec3Da, vec3Db);
ensure("1:angle_between: Fail ", (0 == angle1));
F64 x1 = -1., y1 = -20., z1 = -1.;
vec3Da.clearVec();
vec3Da.setVec(x1,y1,z1);
angle2 = angle_between(vec3Da, vec3Db);
vec3Db.normVec();
vec3Da.normVec();
F64 angle = vec3Db*vec3Da;
angle = acos(angle);
ensure("2:angle_between: Fail ", (angle == angle2));
#endif
}
Ballistics::Ballistics(cv::Mat p1, cv::Mat p2, double alpha, cv::Mat q1, cv::Mat q2) {
_center = line_line_intersection(p1, p2, q1, q2);
/* old_front is the orthonormal vector
* that pointed to the front view of the laser,
* in the first two measurements.
*
* Mathematically speaking, there are two possible ways of computing old_front:
* normalizing p1 - _center, and normalizing p2 - _center.
* In real life, p1 and p2 almost certainly will be given to us with errors.
* We hope to minimize the error by picking the largest of
* (p1 - _center) and (p2 - _center) to normalize.
* (That is, the error relative to _center will probably be smaller
* if we pick the vector that is more distant.)
*/
cv::Mat old_front = p1 - _center;
if( cv::norm(p2 - _center) > cv::norm(old_front) )
old_front = p2 - _center;
// And now normalize.
old_front /= cv::norm(old_front);
// Repeating the same trick for computing _front
_front = q1 - _center;
if( cv::norm(q2 - _center) > cv::norm(_front) )
_front = q2 - _center;
_front /= cv::norm(_front);
double actual_angle = angle_between( old_front, _front );
double projection_angle = angle_of_projection( actual_angle, alpha );
if( alpha > 0 ) {
/* There was a positive rotation from old_front to _front,
* and it is, by exigence, less than pi.
* So, this satisfies the assumptions of compute_rotation_axis.
*/
_up = compute_rotation_axis( old_front, _front, projection_angle );
}
else {
/* There was a negative rotation from old_front to _front.
* This violetes the assumptions made by compute_rotation_axis,
* so we will pretend that there was a positive rotation
* from _front to old_front during the computation of _up.
*/
_up = compute_rotation_axis( _front, old_front, projection_angle );
}
_left = _up.cross(_front);
_up /= cv::norm(_up);
_left /= cv::norm(_left);
}
Platform* Player::choose_next_plat()
{
Vector<float,2> input( 0, 0 );
if( Keyboard::key_down('w') )
input.y() += -1;
if( Keyboard::key_down('s') )
input.y() += 1;
if( Keyboard::key_down('a') )
input.x() += -1;
if( Keyboard::key_down('d') )
input.x() += 1;
Platform* next = 0;
if( magnitude(input) > 0.01f )
{
// Input is rotated to match perspective.
Vector<float,2> direction;
float rotate = -World::zRotDeg * 3.14f/180.f;
direction.x( std::cos(rotate)*input.x() - std::sin(rotate)*input.y() );
direction.y( std::sin(rotate)*input.x() + std::cos(rotate)*input.y() );
// Find platform most in the direction the player is facing.
Platform* nextPlat = 0;
float minAngle = 366;
for( size_t i=0; i < plat->adjacents.size(); i++ )
{
Vector<float,2> d = plat->adjacents[i]->s - plat->s;
float angle = angle_between( direction, d );
if( angle < minAngle ) {
minAngle = angle;
nextPlat = plat->adjacents[i];
}
}
if( nextPlat && minAngle < 3.14 / 4 )
next = nextPlat;
else
next = 0;
}
return next;
}
template<> T CubicHinges<TV2>::slow_elastic_energy(RawArray<const T> angles, RawArray<const TV> restX, RawArray<const TV> X) const {
GEODE_ASSERT(X.size()>=nodes_);
T sum = 0;
const T scale = stiffness;
for (int b=0;b<bends.size();b++) {
int i0,i1,i2;bends[b].get(i0,i1,i2);
const TV x0 = X[i0], x1 = X[i1], x2 = X[i2],
rx0 = restX[i0], rx1 = restX[i1], rx2 = restX[i2];
const T theta = angle_between(x1-x0,x2-x1);
sum += scale/(magnitude(rx1-rx0)+magnitude(rx2-rx1))*sqr(2*sin((theta-angles[b])/2));
// To check this formula, the energy of a radius r polygon with n segments is
//
// E \approx n/2 * 2/(2*2pi r/n) * (2*sin(2pi/n/2))^2
// \approx n/2 * 2n/(4pi r) * (2pi/n)^2
// \approx 4pi^2/(4pi r) = pi/r
}
return sum;
}
/*virtual*/
void LLPullButton::onMouseLeave(S32 x, S32 y, MASK mask)
{
LLButton::onMouseLeave(x, y, mask);
if (mMouseDownTimer.getStarted()) //an user have done a mouse down, if the timer started. see LLButton::handleMouseDown for details
{
const LLVector2 cursor_direction = LLVector2(F32(x), F32(y)) - mLastMouseDown;
/* For now cursor_direction points to the direction of mouse movement
* Need to decide whether should we fire a signal.
* We fire if angle between mDraggingDirection and cursor_direction is less that 45 degree
* Note:
* 0.5 * F_PI_BY_TWO equals to PI/4 radian that equals to angle of 45 degrees
*/
if (angle_between(mDraggingDirection, cursor_direction) < 0.5 * F_PI_BY_TWO)//call if angle < pi/4
{
mClickDraggingSignal(this, LLSD());
}
}
}
int are_contact_samples_continuous(struct ContactSample *aContactSample, struct ContactSample *bContactSample, int tGran, int sGran)
{
double alpha1, alpha2, dist, xdist;
alpha1 =angle_between(aContactSample->gPoint1.x, aContactSample->gPoint1.y, aContactSample->gPoint2.x, aContactSample->gPoint2.y);
alpha2 = MAX(aContactSample->rAngle, alpha1) - MIN(aContactSample->rAngle, alpha1);
dist = distance_in_meter(aContactSample->gPoint1.x, aContactSample->gPoint1.y, aContactSample->gPoint2.x, aContactSample->gPoint2.y);
if(alpha2 == 0)
xdist = sGran - dist;
else if (alpha2 == 180)
xdist = sGran + dist;
else if (alpha2 <= 90 || alpha2 >= 270)
xdist = sqrt(sGran*sGran - dist*dist*sin(alpha2*M_PI/180)*sin(alpha2*M_PI/180)) + dist*cos(alpha2*M_PI/180);
else
xdist = sqrt(sGran*sGran - dist*dist*sin(alpha2*M_PI/180)*sin(alpha2*M_PI/180)) - dist*cos(alpha2*M_PI/180);
if ( xdist >= (MAX(bContactSample->timestamp,aContactSample->timestamp) - MIN(bContactSample->timestamp,aContactSample->timestamp)) * aContactSample->rSpeed / 3.6 )
return 1;
else
return 0;
}
/*
Some explaination of the parameters here is warranted. See:
http://www.w3.org/TR/SVG11/implnote.html#ArcImplementationNotes
Add an arc segment to path, that describes a section of an elliptical
arc from the current point of path to (point_x,point_y), such that:
The arc segment is taken from an elliptical arc of semi major radius
size_x, semi minor radius size_y, where the semi major axis of the
ellipse is rotated by rotation_angle.
If is_large_arc, then the arc segment is selected to be > 180 degrees.
If is_clockwise, then the arc sweeps clockwise.
*/
static void
xps_draw_arc(fz_context *doc, fz_path *path,
float size_x, float size_y, float rotation_angle,
int is_large_arc, int is_clockwise,
float point_x, float point_y)
{
fz_matrix rotmat, revmat;
fz_matrix mtx;
fz_point pt;
float rx, ry;
float x1, y1, x2, y2;
float x1t, y1t;
float cxt, cyt, cx, cy;
float t1, t2, t3;
float sign;
float th1, dth;
pt = fz_currentpoint(doc, path);
x1 = pt.x;
y1 = pt.y;
x2 = point_x;
y2 = point_y;
rx = size_x;
ry = size_y;
if (is_clockwise != is_large_arc)
sign = 1;
else
sign = -1;
fz_rotate(&rotmat, rotation_angle);
fz_rotate(&revmat, -rotation_angle);
/* http://www.w3.org/TR/SVG11/implnote.html#ArcImplementationNotes */
/* Conversion from endpoint to center parameterization */
/* F.6.6.1 -- ensure radii are positive and non-zero */
rx = fabsf(rx);
ry = fabsf(ry);
if (rx < 0.001f || ry < 0.001f || (x1 == x2 && y1 == y2))
{
fz_lineto(doc, path, x2, y2);
return;
}
/* F.6.5.1 */
pt.x = (x1 - x2) / 2;
pt.y = (y1 - y2) / 2;
fz_transform_vector(&pt, &revmat);
x1t = pt.x;
y1t = pt.y;
/* F.6.6.2 -- ensure radii are large enough */
t1 = (x1t * x1t) / (rx * rx) + (y1t * y1t) / (ry * ry);
if (t1 > 1)
{
rx = rx * sqrtf(t1);
ry = ry * sqrtf(t1);
}
/* F.6.5.2 */
t1 = (rx * rx * ry * ry) - (rx * rx * y1t * y1t) - (ry * ry * x1t * x1t);
t2 = (rx * rx * y1t * y1t) + (ry * ry * x1t * x1t);
t3 = t1 / t2;
/* guard against rounding errors; sqrt of negative numbers is bad for your health */
if (t3 < 0) t3 = 0;
t3 = sqrtf(t3);
cxt = sign * t3 * (rx * y1t) / ry;
cyt = sign * t3 * -(ry * x1t) / rx;
/* F.6.5.3 */
pt.x = cxt;
pt.y = cyt;
fz_transform_vector(&pt, &rotmat);
cx = pt.x + (x1 + x2) / 2;
cy = pt.y + (y1 + y2) / 2;
/* F.6.5.4 */
{
fz_point coord1, coord2, coord3, coord4;
coord1.x = 1;
coord1.y = 0;
coord2.x = (x1t - cxt) / rx;
coord2.y = (y1t - cyt) / ry;
coord3.x = (x1t - cxt) / rx;
coord3.y = (y1t - cyt) / ry;
coord4.x = (-x1t - cxt) / rx;
coord4.y = (-y1t - cyt) / ry;
th1 = angle_between(coord1, coord2);
dth = angle_between(coord3, coord4);
if (dth < 0 && !is_clockwise)
dth += (((float)M_PI / 180) * 360);
if (dth > 0 && is_clockwise)
dth -= (((float)M_PI / 180) * 360);
}
fz_pre_scale(fz_pre_rotate(fz_translate(&mtx, cx, cy), rotation_angle), rx, ry);
xps_draw_arc_segment(doc, path, &mtx, th1, th1 + dth, is_clockwise);
fz_lineto(doc, path, point_x, point_y);
}
/*
* NOTE: this implementation follows Standard SVG 1.1 implementation guidelines
* for elliptical arc curves. See Appendix F.6.
*/
void EllipticalArc::_updateCenterAndAngles(bool svg)
{
Point d = initialPoint() - finalPoint();
// TODO move this to SVGElipticalArc?
if (svg)
{
if ( initialPoint() == finalPoint() )
{
_rot_angle = _start_angle = _end_angle = 0;
_center = initialPoint();
_rays = Geom::Point(0,0);
_large_arc = _sweep = false;
return;
}
_rays[X] = std::fabs(_rays[X]);
_rays[Y] = std::fabs(_rays[Y]);
if ( are_near(ray(X), 0) || are_near(ray(Y), 0) )
{
_rays[X] = L2(d) / 2;
_rays[Y] = 0;
_rot_angle = std::atan2(d[Y], d[X]);
_start_angle = 0;
_end_angle = M_PI;
_center = middle_point(initialPoint(), finalPoint());
_large_arc = false;
_sweep = false;
return;
}
}
else
{
if ( are_near(initialPoint(), finalPoint()) )
{
if ( are_near(ray(X), 0) && are_near(ray(Y), 0) )
{
_start_angle = _end_angle = 0;
_center = initialPoint();
return;
}
else
{
THROW_RANGEERROR("initial and final point are the same");
}
}
if ( are_near(ray(X), 0) && are_near(ray(Y), 0) )
{ // but initialPoint != finalPoint
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(X) == 0 && ray(Y) == 0 but initialPoint != finalPoint"
);
}
if ( are_near(ray(Y), 0) )
{
Point v = initialPoint() - finalPoint();
if ( are_near(L2sq(v), 4*ray(X)*ray(X)) )
{
Angle angle(v);
if ( are_near( angle, _rot_angle ) )
{
_start_angle = 0;
_end_angle = M_PI;
_center = v/2 + finalPoint();
return;
}
angle -= M_PI;
if ( are_near( angle, _rot_angle ) )
{
_start_angle = M_PI;
_end_angle = 0;
_center = v/2 + finalPoint();
return;
}
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(Y) == 0 "
"and slope(initialPoint - finalPoint) != rotation_angle "
"and != rotation_angle + PI"
);
}
if ( L2sq(v) > 4*ray(X)*ray(X) )
{
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(Y) == 0 and distance(initialPoint, finalPoint) > 2*ray(X)"
);
}
else
{
THROW_RANGEERROR(
"there is infinite ellipses that satisfy the given constraints: "
"ray(Y) == 0 and distance(initialPoint, finalPoint) < 2*ray(X)"
);
}
}
if ( are_near(ray(X), 0) )
{
Point v = initialPoint() - finalPoint();
if ( are_near(L2sq(v), 4*ray(Y)*ray(Y)) )
{
double angle = std::atan2(v[Y], v[X]);
if (angle < 0) angle += 2*M_PI;
double rot_angle = _rot_angle.radians() + M_PI/2;
if ( !(rot_angle < 2*M_PI) ) rot_angle -= 2*M_PI;
if ( are_near( angle, rot_angle ) )
{
_start_angle = M_PI/2;
_end_angle = 3*M_PI/2;
_center = v/2 + finalPoint();
return;
}
angle -= M_PI;
if ( angle < 0 ) angle += 2*M_PI;
if ( are_near( angle, rot_angle ) )
{
_start_angle = 3*M_PI/2;
_end_angle = M_PI/2;
_center = v/2 + finalPoint();
return;
}
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(X) == 0 "
"and slope(initialPoint - finalPoint) != rotation_angle + PI/2 "
"and != rotation_angle + (3/2)*PI"
);
}
if ( L2sq(v) > 4*ray(Y)*ray(Y) )
{
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(X) == 0 and distance(initialPoint, finalPoint) > 2*ray(Y)"
);
}
else
{
THROW_RANGEERROR(
"there is infinite ellipses that satisfy the given constraints: "
"ray(X) == 0 and distance(initialPoint, finalPoint) < 2*ray(Y)"
);
}
}
}
Rotate rm(_rot_angle);
Affine m(rm);
m[1] = -m[1];
m[2] = -m[2];
Point p = (d / 2) * m;
double rx2 = _rays[X] * _rays[X];
double ry2 = _rays[Y] * _rays[Y];
double rxpy = _rays[X] * p[Y];
double rypx = _rays[Y] * p[X];
double rx2py2 = rxpy * rxpy;
double ry2px2 = rypx * rypx;
double num = rx2 * ry2;
double den = rx2py2 + ry2px2;
assert(den != 0);
double rad = num / den;
Point c(0,0);
if (rad > 1)
{
rad -= 1;
rad = std::sqrt(rad);
if (_large_arc == _sweep) rad = -rad;
c = rad * Point(rxpy / ray(Y), -rypx / ray(X));
_center = c * rm + middle_point(initialPoint(), finalPoint());
}
else if (rad == 1 || svg)
{
double lamda = std::sqrt(1 / rad);
_rays[X] *= lamda;
_rays[Y] *= lamda;
_center = middle_point(initialPoint(), finalPoint());
}
else
{
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints"
);
}
Point sp((p[X] - c[X]) / ray(X), (p[Y] - c[Y]) / ray(Y));
Point ep((-p[X] - c[X]) / ray(X), (-p[Y] - c[Y]) / ray(Y));
Point v(1, 0);
_start_angle = angle_between(v, sp);
double sweep_angle = angle_between(sp, ep);
if (!_sweep && sweep_angle > 0) sweep_angle -= 2*M_PI;
if (_sweep && sweep_angle < 0) sweep_angle += 2*M_PI;
_end_angle = _start_angle;
_end_angle += sweep_angle;
}
Ballistics::rotation_data Ballistics::align(cv::Mat p) {
p -= _center;
rotation_data ret;
// First, compute the rotation in the main axis
{
// Projection on primary rotation plane
cv::Mat pp = project_on_axis( p, _left ) +
project_on_axis( p, _up.cross(_left) );
// Projection of front in the secondary rotation plane
cv::Mat fp = project_on_axis( _front, _left ) +
project_on_axis( _front, _up.cross(_left) );
if( cv::norm(pp) < 1e-8 && cv::norm(fp) < 1e-8 ) {
/* Arbitrarily chosen constants
* This is to avoid division by zero.
*/
ret.main = 0.0;
}
else {
ret.main = angle_between( pp, fp );
/* Note the value returned by angle_between is "unsigned",
* and we need the angle to be "signed"
* (that is, positive is counterclockwise and negative is clockwise
* if the rotation axis points towards you.)
*/
if( cv::norm( pp - rotate_around_axis(_up, fp, -ret.main) )
< cv::norm( pp - rotate_around_axis(_up, fp, ret.main) ) )
ret.main = -ret.main;
// TODO: There must be a smarter way of doing this.
}
} // Rotation in the main axis computed.
/* Now, to compute the rotation in the secondary axis,
* we will first rotate everything through the primary axis.
* We cannot use projections directly because the rotation plane itself
* will be rotated around the primary axis.
*
* Note p remains still; what rotates is _front and _left.
* (_front will be adjusted again later; _left is on its final form.)
*/
_front = rotate_around_axis( _up, _front, ret.main );
_left = rotate_around_axis( _up, _left, ret.main );
// Compute the rotation around the secondary axis
{
/* This one is easier since both p and _front are on the rotation plane.
*/
ret.secondary = angle_between( p, _front );
// Again compute the sign of the rotation
if( cv::norm( p - rotate_around_axis(_left, _front, -ret.secondary) )
< cv::norm( p - rotate_around_axis(_left, _front, ret.secondary) ) )
ret.secondary = -ret.secondary;
} // Rotation in the secondary axis computed.
// Finally, "rotate" _front in the secondary rotation plane.
_front = p / cv::norm(p);
return ret;
}
int main(int argc, char *argv[])
{
int fail = 0;
struct quaternion orientation;
UnitCell *cell;
double asx, asy, asz;
double bsx, bsy, bsz;
double csx, csy, csz;
double asmag, bsmag, csmag;
double als, bes, gas;
double ax, ay, az;
double bx, by, bz;
double cx, cy, cz;
gsl_rng *rng;
rng = gsl_rng_alloc(gsl_rng_mt19937);
cell = cell_new_from_parameters(27.155e-9, 28.155e-9, 10.987e-9,
deg2rad(90.0),
deg2rad(90.0),
deg2rad(120.0));
if ( cell == NULL ) return 1;
orientation = random_quaternion(rng);
cell = cell_rotate(cell, orientation);
cell_get_reciprocal(cell, &asx, &asy, &asz,
&bsx, &bsy, &bsz,
&csx, &csy, &csz);
asmag = sqrt(pow(asx, 2.0) + pow(asy, 2.0) + pow(asz, 2.0));
bsmag = sqrt(pow(bsx, 2.0) + pow(bsy, 2.0) + pow(bsz, 2.0));
csmag = sqrt(pow(csx, 2.0) + pow(csy, 2.0) + pow(csz, 2.0));
als = angle_between(bsx, bsy, bsz, csx, csy, csz);
bes = angle_between(asx, asy, asz, csx, csy, csz);
gas = angle_between(asx, asy, asz, bsx, bsy, bsz);
STATUS("Separation between (100) planes = %5.2f nm\n", 1e9/asmag);
STATUS("Separation between (010) planes = %5.2f nm\n", 1e9/bsmag);
STATUS("Separation between (001) planes = %5.2f nm\n", 1e9/csmag);
STATUS("Angle between (100) and (010) planes = %5.2f deg\n",
rad2deg(gas));
STATUS("Angle between (100) and (001) planes = %5.2f deg\n",
rad2deg(bes));
STATUS("Angle between (010) and (001) planes = %5.2f deg\n",
rad2deg(als));
cell_free(cell);
cell = cell_new();
if ( cell == NULL ) return 1;
cell_set_reciprocal(cell, asx, asy, asz, bsx, bsy, bsz, csx, csy, csz);
cell_print(cell);
cell_get_cartesian(cell, &ax, &ay, &az,
&bx, &by, &bz,
&cx, &cy, &cz);
STATUS("Cell choice 1:\n");
cell_set_cartesian(cell, ax, ay, az,
bx, by, bz,
cx, cy, cz);
cell_print(cell);
STATUS("Cell choice 2:\n");
cell_set_cartesian(cell, bx, by, bz,
-ax-bx, -ay-by, -az-bz,
cx, cy, cz);
cell_print(cell);
STATUS("Cell choice 3:\n");
cell_set_cartesian(cell, -ax-bx, -ay-by, -az-bz,
ax, ay, az,
cx, cy, cz);
cell_print(cell);
gsl_rng_free(rng);
return fail;
}
//-----------------------------------------------------------------------------
// solve()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::solve()
{
// llinfos << llendl;
// llinfos << "LLJointSolverRP3::solve()" << llendl;
//-------------------------------------------------------------------------
// setup joints in their base rotations
//-------------------------------------------------------------------------
mJointA->setRotation( mJointABaseRotation );
mJointB->setRotation( mJointBBaseRotation );
//-------------------------------------------------------------------------
// get joint positions in world space
//-------------------------------------------------------------------------
LLVector3 aPos = mJointA->getWorldPosition();
LLVector3 bPos = mJointB->getWorldPosition();
LLVector3 cPos = mJointC->getWorldPosition();
LLVector3 gPos = mJointGoal->getWorldPosition();
// llinfos << "bPosLocal = " << mJointB->getPosition() << llendl;
// llinfos << "cPosLocal = " << mJointC->getPosition() << llendl;
// llinfos << "bRotLocal = " << mJointB->getRotation() << llendl;
// llinfos << "cRotLocal = " << mJointC->getRotation() << llendl;
// llinfos << "aPos : " << aPos << llendl;
// llinfos << "bPos : " << bPos << llendl;
// llinfos << "cPos : " << cPos << llendl;
// llinfos << "gPos : " << gPos << llendl;
//-------------------------------------------------------------------------
// get the poleVector in world space
//-------------------------------------------------------------------------
LLVector3 poleVec = mPoleVector;
if ( mJointA->getParent() )
{
LLVector4a pole_veca;
pole_veca.load3(mPoleVector.mV);
mJointA->getParent()->getWorldMatrix().rotate(pole_veca,pole_veca);
poleVec.set(pole_veca.getF32ptr());
}
//-------------------------------------------------------------------------
// compute the following:
// vector from A to B
// vector from B to C
// vector from A to C
// vector from A to G (goal)
//-------------------------------------------------------------------------
LLVector3 abVec = bPos - aPos;
LLVector3 bcVec = cPos - bPos;
LLVector3 acVec = cPos - aPos;
LLVector3 agVec = gPos - aPos;
// llinfos << "abVec : " << abVec << llendl;
// llinfos << "bcVec : " << bcVec << llendl;
// llinfos << "acVec : " << acVec << llendl;
// llinfos << "agVec : " << agVec << llendl;
//-------------------------------------------------------------------------
// compute needed lengths of those vectors
//-------------------------------------------------------------------------
F32 abLen = abVec.magVec();
F32 bcLen = bcVec.magVec();
F32 agLen = agVec.magVec();
// llinfos << "abLen : " << abLen << llendl;
// llinfos << "bcLen : " << bcLen << llendl;
// llinfos << "agLen : " << agLen << llendl;
//-------------------------------------------------------------------------
// compute component vector of (A->B) orthogonal to (A->C)
//-------------------------------------------------------------------------
LLVector3 abacCompOrthoVec = abVec - acVec * ((abVec * acVec)/(acVec * acVec));
// llinfos << "abacCompOrthoVec : " << abacCompOrthoVec << llendl;
//-------------------------------------------------------------------------
// compute the normal of the original ABC plane (and store for later)
//-------------------------------------------------------------------------
LLVector3 abcNorm;
if (!mbUseBAxis)
{
if( are_parallel(abVec, bcVec, 0.001f) )
{
// the current solution is maxed out, so we use the axis that is
// orthogonal to both poleVec and A->B
if ( are_parallel(poleVec, abVec, 0.001f) )
{
// ACK! the problem is singular
if ( are_parallel(poleVec, agVec, 0.001f) )
{
// the solutions is also singular
return;
}
else
{
abcNorm = poleVec % agVec;
}
}
else
{
abcNorm = poleVec % abVec;
}
}
else
{
abcNorm = abVec % bcVec;
}
}
else
{
abcNorm = mBAxis * mJointB->getWorldRotation();
}
//-------------------------------------------------------------------------
// compute rotation of B
//-------------------------------------------------------------------------
// angle between A->B and B->C
F32 abbcAng = angle_between(abVec, bcVec);
// vector orthogonal to A->B and B->C
LLVector3 abbcOrthoVec = abVec % bcVec;
if (abbcOrthoVec.magVecSquared() < 0.001f)
{
abbcOrthoVec = poleVec % abVec;
abacCompOrthoVec = poleVec;
}
abbcOrthoVec.normVec();
F32 agLenSq = agLen * agLen;
// angle arm for extension
F32 cosTheta = (agLenSq - abLen*abLen - bcLen*bcLen) / (2.0f * abLen * bcLen);
if (cosTheta > 1.0f)
cosTheta = 1.0f;
else if (cosTheta < -1.0f)
cosTheta = -1.0f;
F32 theta = acos(cosTheta);
LLQuaternion bRot(theta - abbcAng, abbcOrthoVec);
// llinfos << "abbcAng : " << abbcAng << llendl;
// llinfos << "abbcOrthoVec : " << abbcOrthoVec << llendl;
// llinfos << "agLenSq : " << agLenSq << llendl;
// llinfos << "cosTheta : " << cosTheta << llendl;
// llinfos << "theta : " << theta << llendl;
// llinfos << "bRot : " << bRot << llendl;
// llinfos << "theta abbcAng theta-abbcAng: " << theta*180.0/F_PI << " " << abbcAng*180.0f/F_PI << " " << (theta - abbcAng)*180.0f/F_PI << llendl;
//-------------------------------------------------------------------------
// compute rotation that rotates new A->C to A->G
//-------------------------------------------------------------------------
// rotate B->C by bRot
bcVec = bcVec * bRot;
// update A->C
acVec = abVec + bcVec;
LLQuaternion cgRot;
cgRot.shortestArc( acVec, agVec );
// llinfos << "bcVec : " << bcVec << llendl;
// llinfos << "acVec : " << acVec << llendl;
// llinfos << "cgRot : " << cgRot << llendl;
// update A->B and B->C with rotation from C to G
abVec = abVec * cgRot;
bcVec = bcVec * cgRot;
abcNorm = abcNorm * cgRot;
acVec = abVec + bcVec;
//-------------------------------------------------------------------------
// compute the normal of the APG plane
//-------------------------------------------------------------------------
if (are_parallel(agVec, poleVec, 0.001f))
{
// the solution plane is undefined ==> we're done
return;
}
LLVector3 apgNorm = poleVec % agVec;
apgNorm.normVec();
if (!mbUseBAxis)
{
//---------------------------------------------------------------------
// compute the normal of the new ABC plane
// (only necessary if we're NOT using mBAxis)
//---------------------------------------------------------------------
if( are_parallel(abVec, bcVec, 0.001f) )
{
// G is either too close or too far away
// we'll use the old ABCnormal
}
else
{
abcNorm = abVec % bcVec;
}
abcNorm.normVec();
}
//-------------------------------------------------------------------------
// calcuate plane rotation
//-------------------------------------------------------------------------
LLQuaternion pRot;
if ( are_parallel( abcNorm, apgNorm, 0.001f) )
{
if (abcNorm * apgNorm < 0.0f)
{
// we must be PI radians off ==> rotate by PI around agVec
pRot.setQuat(F_PI, agVec);
}
else
{
// we're done
}
}
else
{
pRot.shortestArc( abcNorm, apgNorm );
}
// llinfos << "abcNorm = " << abcNorm << llendl;
// llinfos << "apgNorm = " << apgNorm << llendl;
// llinfos << "pRot = " << pRot << llendl;
//-------------------------------------------------------------------------
// compute twist rotation
//-------------------------------------------------------------------------
LLQuaternion twistRot( mTwist, agVec );
// llinfos << "twist : " << mTwist*180.0/F_PI << llendl;
// llinfos << "agNormVec: " << agNormVec << llendl;
// llinfos << "twistRot : " << twistRot << llendl;
//-------------------------------------------------------------------------
// compute rotation of A
//-------------------------------------------------------------------------
LLQuaternion aRot = cgRot * pRot * twistRot;
//-------------------------------------------------------------------------
// apply the rotations
//-------------------------------------------------------------------------
mJointB->setWorldRotation( mJointB->getWorldRotation() * bRot );
mJointA->setWorldRotation( mJointA->getWorldRotation() * aRot );
}
inline T angle_between(const vec2<S>& a, const vec2<S>& b) {
return angle_between(a.x, a.y, b.x, b.y);
}
void draw_single_road(struct ScreenContext *screenContext, cairo_t *cr, struct Road *aRoad, struct RGBO *fillColor)
{
struct Item *p;
struct Point point, mp;
double width, wRoad;
struct Color lineColor;
double dist, d, angle;
width = aRoad->width/screenContext->meterperpixel;
if(width < 1)
wRoad =1;
else
wRoad = width;
/* draw a road */
lineColor.red = 179.0/255;
lineColor.green = 166.0/255;
lineColor.blue = 147.0/255;
// for test, draw origpoints
if (screenContext->debug) {
p = aRoad->origPoints.head;
gps_to_canvas(&screenContext->awin, ((struct Point*)p->datap)->x, ((struct Point*)p->datap)->y, &point.x, &point.y);
cairo_move_to(cr, point.x-screenContext->scr_x, point.y-screenContext->scr_y);
p = p->next;
while(p!=NULL) {
gps_to_canvas(&screenContext->awin, ((struct Point*)p->datap)->x, ((struct Point*)p->datap)->y, &point.x, &point.y);
cairo_line_to(cr, point.x-screenContext->scr_x, point.y-screenContext->scr_y);
p = p->next;
}
cairo_set_line_width(cr, wRoad+1);
cairo_set_source_rgb(cr, lineColor.red, lineColor.green, lineColor.blue);
cairo_stroke_preserve(cr);
cairo_set_line_width(cr, wRoad);
cairo_set_source_rgba(cr, fillColor->red/255.0, fillColor->green/255.0, fillColor->blue/255.0, fillColor->opacity/255.0);
cairo_stroke(cr);
p = aRoad->origPoints.head;
gps_to_canvas(&screenContext->awin, ((struct Point*)p->datap)->x, ((struct Point*)p->datap)->y, &point.x, &point.y);
cairo_arc(cr, (int)point.x-screenContext->scr_x, (int)point.y-screenContext->scr_y, 2, 0, 2*M_PI) ;
cairo_set_source_rgb(cr, 0.2, 0.2, 0.8);
cairo_fill(cr);
p = p->next;
while(p!=NULL) {
gps_to_canvas(&screenContext->awin, ((struct Point*)p->datap)->x, ((struct Point*)p->datap)->y, &point.x, &point.y);
cairo_arc(cr, (int)point.x-screenContext->scr_x, (int)point.y-screenContext->scr_y, 2, 0, 2*M_PI) ;
cairo_set_source_rgb(cr, 0.2, 0.2, 0.8);
cairo_fill(cr);
p = p->next;
}
}
// draw points on the road
p = aRoad->points.head;
gps_to_canvas(&screenContext->awin, ((struct Point*)p->datap)->x, ((struct Point*)p->datap)->y, &point.x, &point.y);
cairo_move_to(cr, point.x-screenContext->scr_x, point.y-screenContext->scr_y);
dist = 0;
p = p->next;
while(p!=NULL) {
gps_to_canvas(&screenContext->awin, ((struct Point*)p->datap)->x, ((struct Point*)p->datap)->y, &point.x, &point.y);
cairo_line_to(cr, point.x-screenContext->scr_x, point.y-screenContext->scr_y);
d = distance_in_meter(((struct Point*)p->prev->datap)->x, ((struct Point*)p->prev->datap)->y, ((struct Point*)p->datap)->x, ((struct Point*)p->datap)->y);
if (dist < aRoad->length/2 && dist + d > aRoad->length/2) {
gps_to_canvas(&screenContext->awin, (((struct Point*)p->prev->datap)->x+((struct Point*)p->datap)->x)/2, (((struct Point*)p->prev->datap)->y+((struct Point*)p->datap)->y)/2, &mp.x, &mp.y);
angle = angle_between(((struct Point*)p->prev->datap)->x, ((struct Point*)p->prev->datap)->y, ((struct Point*)p->datap)->x, ((struct Point*)p->datap)->y);
}
dist += d;
p = p->next;
}
cairo_set_line_width(cr, wRoad+1);
cairo_set_source_rgb(cr, lineColor.red, lineColor.green, lineColor.blue);
cairo_stroke_preserve(cr);
cairo_set_line_width(cr, wRoad);
cairo_set_source_rgba(cr, fillColor->red/255.0, fillColor->green/255.0, fillColor->blue/255.0, fillColor->opacity/255.0);
cairo_stroke(cr);
if (screenContext->debug) {
p = aRoad->points.head;
gps_to_canvas(&screenContext->awin, ((struct Point*)p->datap)->x, ((struct Point*)p->datap)->y, &point.x, &point.y);
cairo_arc(cr, (int)point.x-screenContext->scr_x, (int)point.y-screenContext->scr_y, 2, 0, 2*M_PI) ;
cairo_set_source_rgb(cr, 0.2, 0.2, 0.8);
cairo_fill(cr);
p = p->next;
while(p!=NULL) {
gps_to_canvas(&screenContext->awin, ((struct Point*)p->datap)->x, ((struct Point*)p->datap)->y, &point.x, &point.y);
cairo_arc(cr, (int)point.x-screenContext->scr_x, (int)point.y-screenContext->scr_y, 2, 0, 2*M_PI) ;
cairo_set_source_rgb(cr, 0.2, 0.2, 0.8);
cairo_fill(cr);
p = p->next;
}
}
// draw arrow
if(screenContext->meterperpixel < 2.5) {
cairo_set_line_width(cr, 1);
cairo_set_source_rgb(cr, lineColor.red, lineColor.green, lineColor.blue);
cairo_move_to(cr, mp.x+cos(M_PI*(90+angle)/180)*MARK_LENGTH/4-screenContext->scr_x, mp.y-sin(M_PI*(90+angle)/180)*MARK_LENGTH/4-screenContext->scr_y);
cairo_line_to(cr, mp.x+cos(M_PI*angle/180)*MARK_LENGTH/2-screenContext->scr_x, mp.y-sin(M_PI*angle/180)*MARK_LENGTH/2-screenContext->scr_y);
cairo_line_to(cr, mp.x+cos(M_PI*(270+angle)/180)*MARK_LENGTH/4-screenContext->scr_x, mp.y-sin(M_PI*(270+angle)/180)*MARK_LENGTH/4-screenContext->scr_y);
cairo_stroke(cr);
cairo_move_to(cr, mp.x+cos(M_PI*angle/180)*MARK_LENGTH/2-screenContext->scr_x, mp.y-sin(M_PI*angle/180)*MARK_LENGTH/2-screenContext->scr_y);
cairo_line_to(cr, mp.x-cos(M_PI*angle/180)*MARK_LENGTH/2-screenContext->scr_x, mp.y+sin(M_PI*angle/180)*MARK_LENGTH/2-screenContext->scr_y);
cairo_stroke(cr);
}
}
/* Change entities velocity in game according to clients keypresses */
static void sv_net_update_clients_entity(client_t *cl)
{
entity *ent;
int accel;
byte dir;
ent = cl->ent; /* Saves typing */
if( ent->mode < ALIVE )
return; /* ent is not allowed to move */
accel = ent->type_info->accel;
if( cl->movemode == NORMAL ) {
if( cl->keypress & FORWARD_BIT ) {
ent->x_v += sin_lookup[ ent->dir ] * accel;
ent->y_v -= cos_lookup[ ent->dir ] * accel;
}
if( cl->keypress & BACK_BIT ) {
ent->x_v -= sin_lookup[ ent->dir ] * accel;
ent->y_v += cos_lookup[ ent->dir ] * accel;
}
if( cl->keypress & LEFT_BIT ) {
dir = ent->dir - DEGREES / 4; /* 1/4 rotation LEFT */
ent->x_v += sin_lookup[ dir ] * accel;
ent->y_v -= cos_lookup[ dir ] * accel;
}
if( cl->keypress & RIGHT_BIT ) {
dir = ent->dir + DEGREES / 4; /* 1/4 rotation RIGHT */
ent->x_v += sin_lookup[ dir ] * accel;
ent->y_v -= cos_lookup[ dir ] * accel;
}
} else { /* Classic movement mode */
int movechange = 0;
if( cl->keypress & FORWARD_BIT )
movechange = 1, ent->y_v = -accel;
if( cl->keypress & BACK_BIT )
movechange = 1, ent->y_v = accel;
if( cl->keypress & LEFT_BIT )
movechange = 1, ent->x_v = -accel;
if( cl->keypress & RIGHT_BIT )
movechange = 1, ent->x_v = accel;
if( movechange ) {
vector_t U, V;
U.x = 0; /* unit vector pointing up (dir = 0) */
U.y = 1;
V.x = 1000 * ent->x_v; /* vector addition of ent velocity */
V.y = -1000 * ent->y_v;
ent->dir = angle_between(U, V);
}
}
/* Have entity (attempt to) fire it's weapon */
if( cl->keypress & B1_BIT ) {
ent->trigger = 1;
} else
ent->trigger = 0;
if( cl->keypress & WEAP_UP_BIT )
ent->weapon_switch = 1;
else if( cl->keypress & WEAP_DN_BIT )
ent->weapon_switch = -1;
else
ent->weapon_switch = 0;
}
bool MaskPolygon::clipPolygon(const FDiff2D center,const double radius)
{
if(radius<=0 || m_polygon.size()<3)
{
return false;
};
FDiff2D s=m_polygon[m_polygon.size()-1];
bool s_inside=clip_insideCircle(s,center,radius);
FDiff2D p;
VectorPolygon newPolygon;
bool needsFinalArc=false;
double angleCovered=0;
double angleCoveredOffset=0;
for(unsigned int i=0;i<m_polygon.size();i++)
{
p=m_polygon[i];
bool p_inside=clip_insideCircle(p,center,radius);
if(p_inside)
{
if(s_inside)
{
//both points inside
newPolygon.push_back(p);
}
else
{
//line crosses circles from outside
std::vector<FDiff2D> points=clip_getIntersectionCircle(p,s,center,radius);
DEBUG_ASSERT(points.size()==1);
angleCovered+=angle_between(s-center,points[0]-center);
if(newPolygon.size()==0)
{
needsFinalArc=true;
angleCoveredOffset=angleCovered;
}
else
{
generateArc(newPolygon,points[0],center,radius,angleCovered<0);
};
newPolygon.push_back(points[0]);
newPolygon.push_back(p);
};
}
else
{
if(!s_inside)
{
//both points outside of circle
std::vector<FDiff2D> points=clip_getIntersectionCircle(s,p,center,radius);
//intersection can only be zero points or 2 points
if(points.size()>1)
{
angleCovered+=angle_between(s-center,points[0]-center);
if(newPolygon.size()==0)
{
needsFinalArc=true;
angleCoveredOffset=angleCovered;
}
else
{
generateArc(newPolygon,points[0],center,radius,angleCovered<0);
};
newPolygon.push_back(points[0]);
newPolygon.push_back(points[1]);
angleCovered=angle_between(points[1]-center,p-center);
}
else
{
angleCovered+=angle_between(s-center,p-center);
};
}
else
{
//line segment intersects circle from inside
std::vector<FDiff2D> points=clip_getIntersectionCircle(s,p,center,radius);
angleCovered=0;
DEBUG_ASSERT(points.size()==1);
newPolygon.push_back(points[0]);
};
};
s=p;
s_inside=p_inside;
};
if(needsFinalArc && newPolygon.size()>1)
{
generateArc(newPolygon,newPolygon[0],center,radius,(angleCovered+angleCoveredOffset)<0);
};
m_polygon=newPolygon;
return (m_polygon.size()>2);
};
// Return the magnitude of a Vec with a direction
// perpendicular to the plane containing this and v
double cross_product(Vec3D<Type> v){
return (magnitude()*v.magnitude()*sin(angle_between(v)));
}
