123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275 |
- /* 2D Vector types implementation. */
- #include <math.h>
- #include "bevec.h"
- BeVec bevec(double x, double y) {
- BeVec result;
- result.x = x;
- result.y = y;
- return result;
- }
- BeVec bevec0() {
- return bevec(0.0, 0.0);
- }
- BeVec bevec_add(BeVec v1, BeVec v2) {
- return bevec(v1.x + v2.x, v1.y + v2.y);
- }
- BeVec bevec_sub(BeVec v1, BeVec v2) {
- return bevec(v1.x - v2.x, v1.y - v2.y);
- }
- BeVec bevec_mul(BeVec v1, double factor) {
- return bevec(v1.x * factor, v1.y * factor);
- }
- BeVec bevec_neg(BeVec v1) {
- return bevec(-v1.x, -v1.y);
- }
- /* Dot product. */
- double bevec_dot(BeVec v1, BeVec v2) {
- return (v1.x * v2.x) + (v1.y * v2.y);
- }
- BeVec bevec_div_unsafe(BeVec v1, double factor) {
- return bevec(v1.x / factor, v1.y / factor);
- }
- BeVec bevec_div(BeVec v1, double factor) {
- if (factor == 0.0) {
- return bevec0();
- }
- return bevec_div_unsafe(v1, factor);
- }
- double bevec_lengthsq(BeVec v1) {
- return (v1.x * v1.x) + (v1.y * v1.y);
- }
- double bevec_length(BeVec v1) {
- return sqrt((v1.x * v1.x) + (v1.y * v1.y));
- }
- BeVec bevec_normalize(BeVec v1) {
- double length = bevec_length(v1);
- return bevec_div(v1, length);
- }
- BeVec bevec_normalize_unsafe(BeVec v1) {
- double length = bevec_length(v1);
- return bevec_div_unsafe(v1, length);
- }
- BeVec bevec_project(BeVec vec , BeVec on) {
- double dot = bevec_dot(vec, on);
- double lengthsq = bevec_lengthsq(on);
- double x = on.x * dot / lengthsq;
- double y = on.y * dot / lengthsq;
- return bevec(x, y);
- }
- BeVec bevec_rightnormal(BeVec vec) {
- return bevec(-vec.y, vec.x);
- }
- BeVec bevec_leftnormal(BeVec vec) {
- return bevec(vec.y, -vec.x);
- }
- double bevec_perproduct(BeVec v1, BeVec v2) {
- return bevec_dot(v1, bevec_rightnormal(v2));
- }
- BeVec bevec_forangle(double angle) {
- return bevec(cos(angle), sin(angle));
- }
- /* Returns a unit vector that makes the given angle a with the X axis. */
- BeVec bevec_for_angle(double a) {
- return bevec(cos(a), sin(a));
- }
- double bevec_toangle(BeVec v1) {
- return atan2(v1.y, v1.x);
- }
- /* Cross product magnitude. The cross product of 2D x,y vectors is
- a 3D vector with a pure z component, so this function returns only the
- magnitude of that z component. */
- double bevec_cross(BeVec self, BeVec other) {
- return self.x * other.y - self.y * other.x;
- }
- /* Perpendicular vector, rotated by 90 degrees, anticlockwise. */
- BeVec bevec_perp(BeVec self) {
- return bevec(-self.y, self.x);
- }
- /* Perpendicular vector, rotated by -90 degrees, clockwise. */
- BeVec bevec_nperp(BeVec self) {
- return bevec(self.y, -self.x);
- }
-
- /* Returns a vector that is the projection of self onto other.
- * Not possible if other has a length 0. */
- BeVec bevec_project_2(BeVec self, BeVec other) {
- double dot_product, other_length;
- dot_product = bevec_dot(self, other);
- other_length = bevec_length(other);
- return bevec_mul(other, dot_product / other_length);
- }
-
- /* Rotates self by other. */
- BeVec bevec_rotate(BeVec self, BeVec other) {
- return bevec(self.x*other.x - self.y*other.y,
- self.x*other.y + self.y*other.x);
- }
-
- /* Inverse rotation. */
- BeVec bevec_unrotate(BeVec self, BeVec other) {
- return bevec(self.x*other.x + self.y*other.y,
- self.x*other.y - self.y*other.x);
- }
-
- /* Linear interpolation on a line between between self and other.
- * Returns a vector that points to a point on the line between self and other. */
- BeVec bevec_lerp(BeVec self, BeVec other, double tx) {
- return bevec_add(bevec_mul(self, 1.0 - tx), bevec_mul(other, tx));
- /* self * (1.0 - tx) + (other * tx) */
- }
-
- /* If the length of the vector is greater than max, this function
- returns a vector with the same direction as self, but with the length
- of that vector limited to max. Otherwise, it returns a copy of self.
- */
- BeVec bevec_cliplength(BeVec self, double m) {
- double len = bevec_length(self);
- if (len <= m) return self;
- return bevec_mul(bevec_normalize(self), m);
- }
- /* Returns the vector between the two points represented by the vectors self and other. */
- BeVec bevec_delta(BeVec self, BeVec other) {
- return bevec_sub(self, other);
- }
- /* Returns the distance between the two points represented by the vectors self and other. */
- double bevec_distance(BeVec self, BeVec other) {
- return bevec_length(bevec_delta(self, other));
- }
- /* Returns the distance squared between the two points represented by the vectors self and other. */
- double bevec_distsq(BeVec self, BeVec other) {
- return bevec_lengthsq(bevec_delta(self, other));
- }
- /* Returns true if the distance between self and other is less than or
- equal to d.
- */
- double bevec_near_p(BeVec self, BeVec other, double d) {
- return bevec_distsq(self, other) <= (d*d);
- }
-
- /* Returns the angle of the vector with the X axis in clockwise
- * direction in radians
- */
- double bevec_angle(BeVec self) {
- return atan2(self.y, self.x);
- }
-
- /* Returns a vector with components self.x, 0 */
- BeVec bevec_xvec(BeVec self) {
- return bevec(self.x, 0.0);
- }
- /* Returns a vector with components 0, self.y */
- BeVec bevec_yvec(BeVec self) {
- return bevec(0.0, self.y);
- }
- /* Returns a vector with fabs applied to the components of vec. */
- BeVec bevec_abs(BeVec self) {
- return bevec(fabs(self.x), fabs(self.y));
- }
-
-
- /*
- Returns the overlap of the x component of self, of other, where
- self and other are centers of line pieces with width rself and rother
- respectivly. Returned as a vector with only an x component with the
- magnitude of overlap.
- BeVec overlap_x(BeVec self, BeVec other,rself, rother)
- xself = self.vector_x
- xother = other.vector_x
- return zero unless xself.near(xother, rself)
- return zero unless xother.near(xself, rother)
- end
- */
- // public domain function by Darel Rex Finley, 2006
- // Determines the intersection point of the line segment defined by points A and B
- // with the line segment defined by points C and D.
- //
- // Returns YES if the intersection point was found, and stores that point in X,Y.
- // Returns NO if there is no determinable intersection point, in which case X,Y will
- // be unmodified.
- /*
- int bevec_intersect_segment(BeVec A, BeVec B, BeVec C, BeVec D, BeVeC * result) {
- double dist_AB, aid_cos, aid_sin, newX, ABpos ;
- // Fail if either line segment is zero-length.
- if (A.x == B.x && A.y == B.y || C.x == D.x && C.y == D.y) return NO;
- // Fail if the segments share an end-point.
- if (A.x==C.x && A.y==C.y || B.x==C.x && B.y==C.y
- || A.x==D.x && A.y==D.y || B.x==D.x && B.y==D.y) {
- return NO; }
- // (1) Translate the system so that point A is on the origin.
- B = bevec_sub(B, A);
- C = bevec_sub(C, A);
- D = bevec_sub(D, A);
-
-
- // Discover the length of segment A-B.
- dist_AB = bevec_length(B);
-
- // (2) Rotate the system so that point B is on the positive X axis.
- aid_Cos=Bx/distAB;
- aid_sin =By/distAB;
- newX=Cx*theCos+Cy* aid_sin;
- Cy =Cy*theCos-Cx* aid_sin; Cx=newX;
- newX=Dx*theCos+Dy* aid_sin;
- Dy =Dy*theCos-Dx* aid_sin; Dx=newX;
- // Fail if segment C-D doesn't cross line A-B.
- if (Cy<0. && Dy<0. || Cy>=0. && Dy>=0.) return NO;
- // (3) Discover the position of the intersection point along line A-B.
- ABpos=Dx+(Cx-Dx)*Dy/(Dy-Cy);
- // Fail if segment C-D crosses line A-B outside of segment A-B.
- if (ABpos<0. || ABpos>distAB) return NO;
- // (4) Apply the discovered position to line A-B in the original coordinate system.
- *X=Ax+ABpos*theCos;
- *Y=Ay+ABpos* aid_sin;
- // Success.
- return YES; }
- */
|