134 lines
3.3 KiB
C
134 lines
3.3 KiB
C
/******************************************************************************
|
|
Copyright (C) 2013 by Hugh Bailey <obs.jim@gmail.com>
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
******************************************************************************/
|
|
|
|
#include <stdlib.h>
|
|
#include "vec2.h"
|
|
#include "vec3.h"
|
|
#include "math-defs.h"
|
|
#include "math-extra.h"
|
|
|
|
void polar_to_cart(struct vec3 *dst, const struct vec3 *v)
|
|
{
|
|
struct vec3 cart;
|
|
float sinx = cosf(v->x);
|
|
float sinx_z = v->z * sinx;
|
|
|
|
cart.x = sinx_z * sinf(v->y);
|
|
cart.z = sinx_z * cosf(v->y);
|
|
cart.y = v->z * sinf(v->x);
|
|
|
|
vec3_copy(dst, &cart);
|
|
}
|
|
|
|
void cart_to_polar(struct vec3 *dst, const struct vec3 *v)
|
|
{
|
|
struct vec3 polar;
|
|
polar.z = vec3_len(v);
|
|
|
|
if (close_float(polar.z, 0.0f, EPSILON)) {
|
|
vec3_zero(&polar);
|
|
} else {
|
|
polar.x = asinf(v->y / polar.z);
|
|
polar.y = atan2f(v->x, v->z);
|
|
}
|
|
|
|
vec3_copy(dst, &polar);
|
|
}
|
|
|
|
void norm_to_polar(struct vec2 *dst, const struct vec3 *norm)
|
|
{
|
|
dst->x = atan2f(norm->x, norm->z);
|
|
dst->y = asinf(norm->y);
|
|
}
|
|
|
|
void polar_to_norm(struct vec3 *dst, const struct vec2 *polar)
|
|
{
|
|
float sinx = sinf(polar->x);
|
|
|
|
dst->x = sinx * cosf(polar->y);
|
|
dst->y = sinx * sinf(polar->y);
|
|
dst->z = cosf(polar->x);
|
|
}
|
|
|
|
float calc_torquef(float val1, float val2, float torque, float min_adjust,
|
|
float t)
|
|
{
|
|
float out = val1;
|
|
float dist;
|
|
bool over;
|
|
|
|
if (close_float(val1, val2, EPSILON))
|
|
return val1;
|
|
|
|
dist = (val2-val1)*torque;
|
|
over = dist > 0.0f;
|
|
|
|
if (over) {
|
|
if (dist < min_adjust) /* prevents from going too slow */
|
|
dist = min_adjust;
|
|
out += dist*t; /* add torque */
|
|
if (out > val2) /* clamp if overshoot */
|
|
out = val2;
|
|
} else {
|
|
if (dist > -min_adjust)
|
|
dist = -min_adjust;
|
|
out += dist*t;
|
|
if (out < val2)
|
|
out = val2;
|
|
}
|
|
|
|
return out;
|
|
}
|
|
|
|
void calc_torque(struct vec3 *dst, const struct vec3 *v1,
|
|
const struct vec3 *v2, float torque, float min_adjust,
|
|
float t)
|
|
{
|
|
struct vec3 line, dir;
|
|
float orig_dist, torque_dist, adjust_dist;
|
|
|
|
if (vec3_close(v1, v2, EPSILON)) {
|
|
vec3_copy(dst, v1);
|
|
return;
|
|
}
|
|
|
|
vec3_sub(&line, v2, v1);
|
|
orig_dist = vec3_len(&line);
|
|
vec3_mulf(&dir, &line, 1.0f/orig_dist);
|
|
|
|
torque_dist = orig_dist*torque; /* use distance to determine speed */
|
|
adjust_dist = torque_dist*t;
|
|
|
|
if (torque_dist < min_adjust) /* prevent from going too slow */
|
|
torque_dist = min_adjust;
|
|
|
|
if (adjust_dist <= (orig_dist-LARGE_EPSILON)) {
|
|
vec3_mulf(dst, &dir, adjust_dist);
|
|
vec3_add(dst, dst, v1); /* add torque */
|
|
} else {
|
|
vec3_copy(dst, v2); /* clamp if overshoot */
|
|
}
|
|
}
|
|
|
|
float rand_float(int positive_only)
|
|
{
|
|
if (positive_only)
|
|
return (float)((double)rand()/(double)RAND_MAX);
|
|
else
|
|
return (float)(((double)rand()/(double)RAND_MAX*2.0)-1.0);
|
|
}
|