diff --git a/src/raymath.h b/src/raymath.h index 96d1e8f70..0221b29af 100644 --- a/src/raymath.h +++ b/src/raymath.h @@ -621,32 +621,36 @@ RMDEF float3 Vector3ToFloatV(Vector3 v) return buffer; } -RMDEF Vector3 Vector3Slerp(Vector3 v1, Vector3 v2, float t, float iradius, float theta) { - +RMDEF Vector3 Vector3Slerp(Vector3 v1, Vector3 v2, float t, float iradius, float theta) +{ if (Vector3LengthSqr(Vector3Subtract(v2,v1))==0.0) return v1; //If our two points are the same, simply return v1 - Vector3 r,a,b,incept,F,G; - Vector3 L=Vector3Subtract(v2,v1); // L=v2-v1 - Vector3 midpoint=Vector3Add(v1,Vector3Scale(L,0.5)); // midpoint=v1+L*0.5 - float D=-1*Vector3DotProduct(L,midpoint); //Solve plane equation for D - if (L.x!=0.0) { //Find intercept of midpoint and x-axis as long as the plane isn't parallel to the x-axis - incept=(Vector3){-D/L.x,0.0,0.0}; + Vector3 r,a,b,incept,f,g; + Vector3 l=Vector3Subtract(v2,v1); // l=v2-v1 + Vector3 midpoint=Vector3Add(v1,Vector3Scale(l,0.5)); // midpoint=v1+l*0.5 + float d=-1*Vector3DotProduct(l,midpoint); //Solve plane equation for d + if (l.x!=0.0) //Find intercept of midpoint and x-axis as long as the plane isn't parallel to the x-axis + { + incept=(Vector3){-d/l.x,0.0,0.0}; } - else if (L.y!=0.0) { //Otherwise use y-axis - incept=(Vector3){0.0,-D/L.y,0.0}; + else if (l.y!=0.0) //Otherwise use y-axis + { + incept=(Vector3){0.0,-d/l.y,0.0}; } - else { //or z-axis. All three cannot be zero because of the first if-statement in routine - incept=(Vector3){0.0,0.0,-D/L.z}; + else + { //or z-axis. All three cannot be zero because of the first if-statement in routine + incept=(Vector3){0.0,0.0,-d/l.z}; } - if (Vector3LengthSqr(Vector3Subtract(incept,midpoint))<0.1) { //If our midpoint and axis intercept are very close together, displace the midpoint slightly so that normalization of F does not result in divide-by-zero or G=0 because F={1,0,0} - midpoint=Vector3Add(v1,Vector3Scale(L,0.49)); + if (Vector3LengthSqr(Vector3Subtract(incept,midpoint))<0.1) //If our midpoint and axis intercept are very close together, displace the midpoint slightly so that normalization of f does not result in divide-by-zero nor g=0 because f={1,0,0} + { + midpoint=Vector3Add(v1,Vector3Scale(l,0.49)); midpoint=Vector3Add(midpoint,Vector3CrossProduct(midpoint,(Vector3){0.2,0.0,0.0})); } - F=Vector3Normalize(Vector3Subtract(incept,midpoint)); // First orthogonal vector - G=Vector3Normalize(Vector3CrossProduct(L,F)); // Second orthogonal vector + f=Vector3Normalize(Vector3Subtract(incept,midpoint)); // First orthogonal vector + g=Vector3Normalize(Vector3CrossProduct(l,f)); // Second orthogonal vector - Vector3 perp=Vector3Add(midpoint,Vector3Scale(Vector3Add( Vector3Scale(F,cos(theta)),Vector3Scale(G,sin(theta))),iradius)); //perp=midpoint+(F*cos(theta)+G*sin(theta))*iradius + Vector3 perp=Vector3Add(midpoint,Vector3Scale(Vector3Add( Vector3Scale(f,cos(theta)),Vector3Scale(g,sin(theta))),iradius)); //perp=midpoint+(f*cos(theta)+g*sin(theta))*iradius a=Vector3Subtract(v1,perp); //translate coordinates to center at perp b=Vector3Subtract(v2,perp); float om=acos( Vector3DotProduct(a,b) / (Vector3Length(a)*Vector3Length(b)) ); //Apply slerp formula for points at origin