1 /* 2 * Geometric operations. 3 * 4 * Copyright (C) 2013 Paul Boddie 5 * 6 * This program is free software; you can redistribute it and/or modify 7 * it under the terms of the GNU General Public License as published by 8 * the Free Software Foundation; either version 2 of the License, or 9 * (at your option) any later version. 10 */ 11 12 #include "geo.h" 13 14 /** 15 * Return a value within the given magnitude. 16 */ 17 double within(double value, double mag) 18 { 19 double offset = fmod(value, mag); 20 int num = (int) fdiv(value, mag); 21 22 if (num % 2 == 0) 23 return offset; 24 else if (offset > 0) 25 return offset - mag; 26 else 27 return offset + mag; 28 } 29 30 /** 31 * Return whether the given vector is null. 32 */ 33 bool vectorf_null(const vectorf *in) 34 { 35 return ((in->x == 0) && (in->y == 0) && (in->z == 0)); 36 } 37 38 /** 39 * Reset the given vector to have zero magnitude. 40 */ 41 void vectorf_reset(vectorf *in) 42 { 43 in->x = 0; in->y = 0; in->z = 0; 44 } 45 46 /** 47 * Return the cross product of the two input vectors in the output vector 48 * provided. 49 */ 50 void vectorf_cross(const vectorf *in1, const vectorf *in2, vectorf *out) 51 { 52 out->x = in1->y * in2->z - in1->z * in2->y; 53 out->y = in1->z * in2->x - in1->x * in2->z; 54 out->z = in1->x * in2->y - in1->y * in2->x; 55 } 56 57 /** 58 * Return the dot product of the two input vectors. 59 */ 60 double vectorf_dot(const vectorf *in1, const vectorf *in2) 61 { 62 return in1->x * in2->x + in1->y * in2->y + in1->z * in2->z; 63 } 64 65 /** 66 * Return the magnitude of the given vector. 67 */ 68 double vectorf_mag(const vectorf *in) 69 { 70 return sqrt(pow(in->x, 2) + pow(in->y, 2) + pow(in->z, 2)); 71 } 72 73 /** 74 * Normalise the given vector. 75 */ 76 void vectorf_normalise(vectorf *in, vectorf *out) 77 { 78 double mag = vectorf_mag(in); 79 80 out->x = in->x / mag; 81 out->y = in->y / mag; 82 out->z = in->z / mag; 83 } 84 85 /** 86 * Rotate the plane defined by the given input vectors by the given angle, 87 * in radians, modifying the input vectors to define the resulting plane. 88 */ 89 void plane_rotate(vectorf *in1, vectorf *in2, double angle) 90 { 91 double c = cos(angle), s = sin(angle); 92 vectorf out1, out2; 93 94 /* Rotation around the plane normal with contributions from both plane 95 vectors. */ 96 97 out1.x = c * in1->x + s * in2->x; 98 out1.y = c * in1->y + s * in2->y; 99 out1.z = c * in1->z + s * in2->z; 100 101 /* The second plane vector is 90 degrees (pi/2) ahead of the first: 102 cos(angle + pi/2) == -sin(angle) 103 sin(angle + pi/2) == cos(angle) */ 104 105 out2.x = c * in2->x - s * in1->x; 106 out2.y = c * in2->y - s * in1->y; 107 out2.z = c * in2->z - s * in1->z; 108 109 *in1 = out1; 110 *in2 = out2; 111 } 112 113 /** 114 * Populate the output vector with the negated input vector. 115 */ 116 void vectorf_negate(vectorf *in, vectorf *out) 117 { 118 out->x = -in->x; 119 out->y = -in->y; 120 out->z = -in->z; 121 } 122 123 /** 124 * Return the direction of the given vector in radians. 125 */ 126 double vectorf_direction(vectorf *in) 127 { 128 return atan2(in->x, in->z); 129 } 130 131 /** 132 * Return the elevation of the given unit vector in radians. 133 */ 134 double vectorf_elevation(vectorf *in) 135 { 136 return asin(in->y); 137 } 138 139 /** 140 * Obtain the direction vector for the given direction and elevation. 141 */ 142 void vectorf_polar(double direction, double elevation, vectorf *out) 143 { 144 out->x = sin(direction) * cos(elevation); 145 out->y = sin(elevation); 146 out->z = cos(direction) * cos(elevation); 147 } 148 149 /** 150 * Convert the input vector expressed in terms of the given axis vectors into 151 * the global coordinate space. 152 */ 153 void vectorf_convert(const vectorf *in, const vectorf *devicex, const vectorf *devicey, const vectorf *devicez, vectorf *out) 154 { 155 out->x = in->x * devicex->x + in->y * devicey->x + in->z * devicez->x; 156 out->y = in->x * devicex->y + in->y * devicey->y + in->z * devicez->y; 157 out->z = in->x * devicex->z + in->y * devicey->z + in->z * devicez->z; 158 } 159 160 /** 161 * Convert the input vector expressed in terms of the global coordinate space 162 * into a vector expressed in terms of the given axis vectors. 163 */ 164 void vectorf_convert_into(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, vectorf *out) 165 { 166 out->x = vectorf_dot(in, x); 167 out->y = vectorf_dot(in, y); 168 out->z = vectorf_dot(in, z); 169 } 170 171 /** 172 * Return the tilt/roll of the given vector in radians projected into the given 173 * plane. 174 */ 175 double vectorf_tilt_in_plane(const vectorf *in, const vectorf *x, const vectorf *y) 176 { 177 vectorf v, z, y2; 178 179 vectorf_cross(x, y, &z); 180 vectorf_normalise(&z, &z); 181 vectorf_cross(&z, x, &y2); 182 vectorf_convert_into(in, x, &y2, &z, &v); 183 return atan2(v.y, v.x); 184 } 185 186 /** 187 * Return the tilt/roll of the given vector in radians projected into the given 188 * space. 189 */ 190 double vectorf_tilt_in_plane_with_axis(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z) 191 { 192 vectorf v, z2, y2; 193 194 vectorf_cross(x, y, &z2); 195 if (vectorf_dot(z, &z2) < 0) 196 vectorf_negate(&z2, &z2); 197 vectorf_cross(&z2, x, &y2); 198 vectorf_convert_into(in, x, &y2, &z2, &v); 199 return atan2(v.y, v.x); 200 } 201 202 /** 203 * Rotate a vector around the given z axis by the given angle in radians. 204 */ 205 void vectorf_rotate_in_space(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, double angle, vectorf *out) 206 { 207 vectorf v; 208 double tilt, c, s, vz, mag; 209 210 /* Project the vector into the xy plane. */ 211 212 vectorf_convert_into(in, x, y, z, &v); 213 214 /* Remember the z value before obtaining a two dimensional vector. */ 215 216 vz = v.z; 217 v.z = 0; 218 mag = vectorf_mag(&v); 219 220 /* Get the current angle. */ 221 222 tilt = atan2(v.y, v.x); 223 224 /* Calculate the rotated vector. */ 225 226 c = cos(tilt + angle); 227 s = sin(tilt + angle); 228 229 out->x = mag * (c * x->x + s * y->x) + vz * z->x; 230 out->y = mag * (c * x->y + s * y->y) + vz * z->y; 231 out->z = mag * (c * x->z + s * y->z) + vz * z->z; 232 }