/* 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))); }