1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
1.2 +++ b/geo.c Mon Oct 14 13:02:19 2013 +0000
1.3 @@ -0,0 +1,232 @@
1.4 +/*
1.5 + * Geometric operations.
1.6 + *
1.7 + * Copyright (C) 2013 Paul Boddie
1.8 + *
1.9 + * This program is free software; you can redistribute it and/or modify
1.10 + * it under the terms of the GNU General Public License as published by
1.11 + * the Free Software Foundation; either version 2 of the License, or
1.12 + * (at your option) any later version.
1.13 + */
1.14 +
1.15 +#include "geo.h"
1.16 +
1.17 +/**
1.18 + * Return a value within the given magnitude.
1.19 + */
1.20 +double within(double value, double mag)
1.21 +{
1.22 + double offset = fmod(value, mag);
1.23 + int num = (int) fdiv(value, mag);
1.24 +
1.25 + if (num % 2 == 0)
1.26 + return offset;
1.27 + else if (offset > 0)
1.28 + return offset - mag;
1.29 + else
1.30 + return offset + mag;
1.31 +}
1.32 +
1.33 +/**
1.34 + * Return whether the given vector is null.
1.35 + */
1.36 +bool vectorf_null(const vectorf *in)
1.37 +{
1.38 + return ((in->x == 0) && (in->y == 0) && (in->z == 0));
1.39 +}
1.40 +
1.41 +/**
1.42 + * Reset the given vector to have zero magnitude.
1.43 + */
1.44 +void vectorf_reset(vectorf *in)
1.45 +{
1.46 + in->x = 0; in->y = 0; in->z = 0;
1.47 +}
1.48 +
1.49 +/**
1.50 + * Return the cross product of the two input vectors in the output vector
1.51 + * provided.
1.52 + */
1.53 +void vectorf_cross(const vectorf *in1, const vectorf *in2, vectorf *out)
1.54 +{
1.55 + out->x = in1->y * in2->z - in1->z * in2->y;
1.56 + out->y = in1->z * in2->x - in1->x * in2->z;
1.57 + out->z = in1->x * in2->y - in1->y * in2->x;
1.58 +}
1.59 +
1.60 +/**
1.61 + * Return the dot product of the two input vectors.
1.62 + */
1.63 +double vectorf_dot(const vectorf *in1, const vectorf *in2)
1.64 +{
1.65 + return in1->x * in2->x + in1->y * in2->y + in1->z * in2->z;
1.66 +}
1.67 +
1.68 +/**
1.69 + * Return the magnitude of the given vector.
1.70 + */
1.71 +double vectorf_mag(const vectorf *in)
1.72 +{
1.73 + return sqrt(pow(in->x, 2) + pow(in->y, 2) + pow(in->z, 2));
1.74 +}
1.75 +
1.76 +/**
1.77 + * Normalise the given vector.
1.78 + */
1.79 +void vectorf_normalise(vectorf *in, vectorf *out)
1.80 +{
1.81 + double mag = vectorf_mag(in);
1.82 +
1.83 + out->x = in->x / mag;
1.84 + out->y = in->y / mag;
1.85 + out->z = in->z / mag;
1.86 +}
1.87 +
1.88 +/**
1.89 + * Rotate the plane defined by the given input vectors by the given angle,
1.90 + * in radians, modifying the input vectors to define the resulting plane.
1.91 + */
1.92 +void plane_rotate(vectorf *in1, vectorf *in2, double angle)
1.93 +{
1.94 + double c = cos(angle), s = sin(angle);
1.95 + vectorf out1, out2;
1.96 +
1.97 + /* Rotation around the plane normal with contributions from both plane
1.98 + vectors. */
1.99 +
1.100 + out1.x = c * in1->x + s * in2->x;
1.101 + out1.y = c * in1->y + s * in2->y;
1.102 + out1.z = c * in1->z + s * in2->z;
1.103 +
1.104 + /* The second plane vector is 90 degrees (pi/2) ahead of the first:
1.105 + cos(angle + pi/2) == -sin(angle)
1.106 + sin(angle + pi/2) == cos(angle) */
1.107 +
1.108 + out2.x = c * in2->x - s * in1->x;
1.109 + out2.y = c * in2->y - s * in1->y;
1.110 + out2.z = c * in2->z - s * in1->z;
1.111 +
1.112 + *in1 = out1;
1.113 + *in2 = out2;
1.114 +}
1.115 +
1.116 +/**
1.117 + * Populate the output vector with the negated input vector.
1.118 + */
1.119 +void vectorf_negate(vectorf *in, vectorf *out)
1.120 +{
1.121 + out->x = -in->x;
1.122 + out->y = -in->y;
1.123 + out->z = -in->z;
1.124 +}
1.125 +
1.126 +/**
1.127 + * Return the direction of the given vector in radians.
1.128 + */
1.129 +double vectorf_direction(vectorf *in)
1.130 +{
1.131 + return atan2(in->x, in->z);
1.132 +}
1.133 +
1.134 +/**
1.135 + * Return the elevation of the given unit vector in radians.
1.136 + */
1.137 +double vectorf_elevation(vectorf *in)
1.138 +{
1.139 + return asin(in->y);
1.140 +}
1.141 +
1.142 +/**
1.143 + * Obtain the direction vector for the given direction and elevation.
1.144 + */
1.145 +void vectorf_polar(double direction, double elevation, vectorf *out)
1.146 +{
1.147 + out->x = sin(direction) * cos(elevation);
1.148 + out->y = sin(elevation);
1.149 + out->z = cos(direction) * cos(elevation);
1.150 +}
1.151 +
1.152 +/**
1.153 + * Convert the input vector expressed in terms of the given axis vectors into
1.154 + * the global coordinate space.
1.155 + */
1.156 +void vectorf_convert(const vectorf *in, const vectorf *devicex, const vectorf *devicey, const vectorf *devicez, vectorf *out)
1.157 +{
1.158 + out->x = in->x * devicex->x + in->y * devicey->x + in->z * devicez->x;
1.159 + out->y = in->x * devicex->y + in->y * devicey->y + in->z * devicez->y;
1.160 + out->z = in->x * devicex->z + in->y * devicey->z + in->z * devicez->z;
1.161 +}
1.162 +
1.163 +/**
1.164 + * Convert the input vector expressed in terms of the global coordinate space
1.165 + * into a vector expressed in terms of the given axis vectors.
1.166 + */
1.167 +void vectorf_convert_into(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, vectorf *out)
1.168 +{
1.169 + out->x = vectorf_dot(in, x);
1.170 + out->y = vectorf_dot(in, y);
1.171 + out->z = vectorf_dot(in, z);
1.172 +}
1.173 +
1.174 +/**
1.175 + * Return the tilt/roll of the given vector in radians projected into the given
1.176 + * plane.
1.177 + */
1.178 +double vectorf_tilt_in_plane(const vectorf *in, const vectorf *x, const vectorf *y)
1.179 +{
1.180 + vectorf v, z, y2;
1.181 +
1.182 + vectorf_cross(x, y, &z);
1.183 + vectorf_normalise(&z, &z);
1.184 + vectorf_cross(&z, x, &y2);
1.185 + vectorf_convert_into(in, x, &y2, &z, &v);
1.186 + return atan2(v.y, v.x);
1.187 +}
1.188 +
1.189 +/**
1.190 + * Return the tilt/roll of the given vector in radians projected into the given
1.191 + * space.
1.192 + */
1.193 +double vectorf_tilt_in_plane_with_axis(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z)
1.194 +{
1.195 + vectorf v, z2, y2;
1.196 +
1.197 + vectorf_cross(x, y, &z2);
1.198 + if (vectorf_dot(z, &z2) < 0)
1.199 + vectorf_negate(&z2, &z2);
1.200 + vectorf_cross(&z2, x, &y2);
1.201 + vectorf_convert_into(in, x, &y2, &z2, &v);
1.202 + return atan2(v.y, v.x);
1.203 +}
1.204 +
1.205 +/**
1.206 + * Rotate a vector around the given z axis by the given angle in radians.
1.207 + */
1.208 +void vectorf_rotate_in_space(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, double angle, vectorf *out)
1.209 +{
1.210 + vectorf v;
1.211 + double tilt, c, s, vz, mag;
1.212 +
1.213 + /* Project the vector into the xy plane. */
1.214 +
1.215 + vectorf_convert_into(in, x, y, z, &v);
1.216 +
1.217 + /* Remember the z value before obtaining a two dimensional vector. */
1.218 +
1.219 + vz = v.z;
1.220 + v.z = 0;
1.221 + mag = vectorf_mag(&v);
1.222 +
1.223 + /* Get the current angle. */
1.224 +
1.225 + tilt = atan2(v.y, v.x);
1.226 +
1.227 + /* Calculate the rotated vector. */
1.228 +
1.229 + c = cos(tilt + angle);
1.230 + s = sin(tilt + angle);
1.231 +
1.232 + out->x = mag * (c * x->x + s * y->x) + vz * z->x;
1.233 + out->y = mag * (c * x->y + s * y->y) + vz * z->y;
1.234 + out->z = mag * (c * x->z + s * y->z) + vz * z->z;
1.235 +}