1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
|
/********************************************************************
* Description: spherical_arc.h
*
* A simple spherical linear interpolation library and related functions.
*
* Author: Robert W. Ellenberg
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2014 All rights reserved.
*
********************************************************************/
#ifndef SPHERICAL_ARC_H
#define SPHERICAL_ARC_H
#include "posemath.h"
#define ARC_POS_EPSILON 1e-12
#define ARC_MIN_RADIUS 1e-12
#define ARC_MIN_ANGLE 1e-6
//FIXME relate this to cornering acceleration?
#define ARC_ABS_ERR 5e-4
#define ARC_REL_ERR 5e-4
typedef struct {
// Three defining points for the arc
PmCartesian start;
PmCartesian end;
PmCartesian center;
// Relative vectors from center to start and center to end
// These are cached here since they'll be reused during SLERP
PmCartesian rStart;
PmCartesian rEnd;
PmCartesian uTan; /* Tangent vector at start of arc (copied from
prev. tangent line)*/
PmCartesian binormal;
double radius;
double spiral;
// Angle that the arc encloses
double angle;
double Sangle;
double line_length;
} SphericalArc;
int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start,
PmCartesian const * const end, PmCartesian const * const center);
int arcInitFromVectors(SphericalArc * const arc, PmCartesian const * const vec0,
PmCartesian const * const vec1,
PmCartesian const * const center);
int arcPoint(SphericalArc const * const arc, double angle_in, PmCartesian * const out);
int arcNormalizedSlerp(SphericalArc const * const arc, double t, PmCartesian * const out);
int arcLength(SphericalArc const * const arc, double * const length);
int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1,
PmCartLine const * const line2, double radius,
double blend_dist, double center_dist, PmCartesian * const start, PmCartesian * const end, int consume);
int arcConvexTest(PmCartesian const * const center,
PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir);
int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end);
#endif
|
