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