blendmath.h
1 /******************************************************************** 2 * Description: blendmath.h 3 * Circular arc blend math functions 4 * 5 * Author: Robert W. Ellenberg 6 * License: GPL Version 2 7 * System: Linux 8 * 9 * Copyright (c) 2014 All rights reserved. 10 * 11 * Last change: 12 ********************************************************************/ 13 #ifndef BLENDMATH_H 14 #define BLENDMATH_H 15 16 #include "posemath.h" 17 #include "tc_types.h" 18 19 #define BLEND_ACC_RATIO_TANGENTIAL 0.5 20 #define BLEND_ACC_RATIO_NORMAL (pmSqrt(1.0 - pmSq(BLEND_ACC_RATIO_TANGENTIAL))) 21 #define BLEND_KINK_FACTOR 0.25 22 23 typedef enum { 24 BLEND_NONE, 25 BLEND_LINE_LINE, 26 BLEND_LINE_ARC, 27 BLEND_ARC_LINE, 28 BLEND_ARC_ARC, 29 } blend_type_t; 30 31 /** 32 * 3D Input geometry for a spherical blend arc. 33 * This structure contains all of the basic geometry in 3D for a blend arc. 34 */ 35 typedef struct { 36 PmCartesian u1; /* unit vector along line 1 */ 37 PmCartesian u2; /* unit vector along line 2 */ 38 PmCartesian P; /* Intersection point */ 39 PmCartesian normal; /* normal unit vector to plane containing lines */ 40 PmCartesian binormal; /* binormal unit vector to plane containing lines */ 41 PmCartesian u_tan1; /* Actual tangent vector to 1 (used for arcs only) */ 42 PmCartesian u_tan2; /* Actual tangent vector to 2 (used for arcs only) */ 43 PmCartesian center1; /* Local approximation of center for arc 1 */ 44 PmCartesian center2; /* Local approximation of center for arc 2 */ 45 double radius1; /* Local approximation of radius */ 46 double radius2; 47 double theta_tan; 48 double v_max1; /* maximum velocity in direction u_tan1 */ 49 double v_max2; /* maximum velocity in direction u_tan2 */ 50 51 } BlendGeom3; 52 53 /** 54 * 9D Input geometry for a spherical blend arc. 55 */ 56 #ifdef BLEND_9D 57 typedef struct { 58 //Not implemented yet 59 } BlendGeom9; 60 #endif 61 62 63 /** 64 * Blend arc parameters (abstracted). 65 * This structure holds blend arc parameters that have been abstracted from the 66 * physical geometry. This data is used to find the maximum radius given the 67 * constraints on the blend. By abstracting the parameters from the geometry, 68 * the same calculations can be used with any input geometry (lines, arcs, 6 or 69 * 9 dimensional lines). 70 */ 71 typedef struct { 72 double tolerance; /* Net blend tolerance (min of line 1 and 2) */ 73 double L1; /* Available part of line 1 to blend over */ 74 double L2; /* Available part of line 2 to blend over */ 75 double v_req; /* requsted velocity for the blend arc */ 76 double a_max; /* max acceleration allowed for blend */ 77 78 /* These fields are considered "output", and may be refactored into a 79 * separate structure in the future */ 80 81 double theta; /* Intersection angle, half of angle between -u1 and u2 */ 82 double phi; /* supplement of intersection angle, angle between u1 and u2 */ 83 double a_n_max; /* max normal acceleration allowed */ 84 85 double R_plan; /* planned radius for blend arc */ 86 double d_plan; /* distance along each line to arc endpoints */ 87 88 double v_goal; /* desired velocity at max feed override */ 89 double v_plan; /* planned max velocity at max feed override */ 90 double v_actual; /* velocity at feedscale = 1.0 */ 91 double s_arc; /* arc length */ 92 int consume; /* Consume the previous segment */ 93 double line_length; 94 //Arc specific stuff 95 int convex1; 96 int convex2; 97 double phi1_max; 98 double phi2_max; 99 100 } BlendParameters; 101 102 103 /** 104 * Output geometry in 3D. 105 * Stores the three points representing a simple 3D spherical arc. 106 */ 107 typedef struct { 108 PmCartesian arc_start; /* start point for blend arc */ 109 PmCartesian arc_end; /* end point for blend arc */ 110 PmCartesian arc_center; /* center point for blend arc */ 111 double trim1; /* length (line) or angle (arc) to cut from prev_tc */ 112 double trim2; /* length (line) or angle (arc) to cut from tc */ 113 } BlendPoints3; 114 115 116 117 #ifdef BLEND_9D 118 typedef struct { 119 //Not implemented yet 120 } BlendPoints9; 121 #endif 122 123 double findMaxTangentAngle(double v, double acc, double cycle_time); 124 125 double findKinkAccel(double kink_angle, double v_plan, double cycle_time); 126 127 double fsign(double f); 128 129 int clip_min(double * const x, double min); 130 131 int clip_max(double * const x, double max); 132 133 double saturate(double x, double max); 134 135 double bisaturate(double x, double max, double min); 136 137 int sat_inplace(double * const x, double max); 138 139 int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const arc, BlendGeom3 const * const geom, BlendParameters const * const param, double cycle_time, int at_end); 140 141 int findIntersectionAngle(PmCartesian const * const u1, 142 PmCartesian const * const u2, double * const theta); 143 144 double pmCartMin(PmCartesian const * const in); 145 146 int calculateInscribedDiameter(PmCartesian const * const normal, 147 PmCartesian const * const bounds, double * const diameter); 148 149 int findAccelScale(PmCartesian const * const acc, 150 PmCartesian const * const bounds, 151 PmCartesian * const scale); 152 153 int pmUnitCartsColinear(PmCartesian const * const u1, 154 PmCartesian const * const u2); 155 156 int pmCartCartParallel(PmCartesian const * const u1, 157 PmCartesian const * const u2, 158 double tol); 159 160 int pmCartCartAntiParallel(PmCartesian const * const u1, 161 PmCartesian const * const u2, 162 double tol); 163 164 int pmCircLineCoplanar(PmCircle const * const circ, 165 PmCartLine const * const line, double tol); 166 167 int blendCoplanarCheck(PmCartesian const * const normal, 168 PmCartesian const * const u1_tan, 169 PmCartesian const * const u2_tan, 170 double tol); 171 172 int blendCalculateNormals3(BlendGeom3 * const geom); 173 174 int blendComputeParameters(BlendParameters * const param); 175 176 int blendCheckConsume(BlendParameters * const param, 177 BlendPoints3 const * const points, 178 TC_STRUCT const * const prev_tc, int gap_cycles); 179 180 int blendFindPoints3(BlendPoints3 * const points, BlendGeom3 const * const geom, 181 BlendParameters const * const param); 182 183 int blendGeom3Init(BlendGeom3 * const geom, 184 TC_STRUCT const * const prev_tc, 185 TC_STRUCT const * const tc); 186 187 int blendParamKinematics(BlendGeom3 * const geom, 188 BlendParameters * const param, 189 TC_STRUCT const * const prev_tc, 190 TC_STRUCT const * const tc, 191 PmCartesian const * const acc_bound, 192 PmCartesian const * const vel_bound, 193 double maxFeedScale); 194 195 int blendInit3FromLineLine(BlendGeom3 * const geom, BlendParameters * const param, 196 TC_STRUCT const * const prev_tc, 197 TC_STRUCT const * const tc, 198 PmCartesian const * const acc_bound, 199 PmCartesian const * const vel_bound, 200 double maxFeedScale); 201 202 int blendInit3FromLineArc(BlendGeom3 * const geom, BlendParameters * const param, 203 TC_STRUCT const * const prev_tc, 204 TC_STRUCT const * const tc, 205 PmCartesian const * const acc_bound, 206 PmCartesian const * const vel_bound, 207 double maxFeedScale); 208 209 int blendInit3FromArcLine(BlendGeom3 * const geom, BlendParameters * const param, 210 TC_STRUCT const * const prev_tc, 211 TC_STRUCT const * const tc, 212 PmCartesian const * const acc_bound, 213 PmCartesian const * const vel_bound, 214 double maxFeedScale); 215 216 int blendInit3FromArcArc(BlendGeom3 * const geom, BlendParameters * const param, 217 TC_STRUCT const * const prev_tc, 218 TC_STRUCT const * const tc, 219 PmCartesian const * const acc_bound, 220 PmCartesian const * const vel_bound, 221 double maxFeedScale); 222 223 int blendArcArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, 224 BlendParameters * const param, BlendGeom3 const * const geom, 225 PmCircle const * const circ1, PmCircle const * const circ2); 226 227 int blendLineArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, 228 BlendParameters * const param, BlendGeom3 const * const geom, 229 PmCartLine const * const line1, PmCircle const * const circ2); 230 231 int blendArcLinePostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, 232 BlendParameters * const param, BlendGeom3 const * const geom, 233 PmCircle const * const circ1, PmCartLine const * const line2); 234 235 int arcFromBlendPoints3(SphericalArc * const arc, BlendPoints3 const * const points, 236 BlendGeom3 const * const geom, BlendParameters const * const param); 237 238 //Not implemented yet 239 int blendGeom3Print(BlendGeom3 const * const geom); 240 int blendParamPrint(BlendParameters const * const param); 241 int blendPoints3Print(BlendPoints3 const * const points); 242 243 double pmCartAbsMax(PmCartesian const * const v); 244 245 typedef struct { 246 double v_max; 247 double acc_ratio; 248 } PmCircleLimits; 249 250 PmCircleLimits pmCircleActualMaxVel(const PmCircle *circle, 251 double v_max_nominal, 252 double a_max_nominal); 253 254 int findSpiralArcLengthFit(PmCircle const * const circle, 255 SpiralArcLengthFit * const fit); 256 int pmCircleAngleFromProgress(PmCircle const * const circle, 257 SpiralArcLengthFit const * const fit, 258 double progress, 259 double * const angle); 260 double pmCircleEffectiveMinRadius(const PmCircle *circle); 261 262 static inline double findVPeak(double a_t_max, double distance) 263 { 264 return pmSqrt(a_t_max * distance); 265 } 266 #endif