-
Amira Abdel-Rahman authoredAmira Abdel-Rahman authored
vector.jl 16.39 KiB
# Amira Abdel-Rahman
# (c) Massachusetts Institute of Technology 2020
struct Vector3
x::Float64
y::Float64
z::Float64
function Vector3()
x=0.0
y=0.0
z=0.0
new(x,y,z)
end
function Vector3(x,y,z)
new(x,y,z)
end
end
struct Quaternion
x::Float64
y::Float64
z::Float64
w::Float64
function Quaternion()
x=0.0
y=0.0
z=0.0
w=1.0
new(x,y,z,w)
end
function Quaternion(x,y,z,w)
new(x,y,z,w)
end
end
struct RotationMatrix
te1::Float64
te2::Float64
te3::Float64
te4::Float64
te5::Float64
te6::Float64
te7::Float64
te8::Float64
te9::Float64
te10::Float64
te11::Float64
te12::Float64
te13::Float64
te14::Float64
te15::Float64
te16::Float64
function RotationMatrix()
te1 =0.0
te2 =0.0
te3 =0.0
te4 =0.0
te5 =0.0
te6 =0.0
te7 =0.0
te8 =0.0
te9 =0.0
te10=0.0
te11=0.0
te12=0.0
te13=0.0
te14=0.0
te15=0.0
te16=0.0
new(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16) end
function RotationMatrix(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
new(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
end
end
+(f::Vector3, g::Vector3)=Vector3(f.x+g.x , f.y+g.y,f.z+g.z )
-(f::Vector3, g::Vector3)=Vector3(f.x-g.x , f.y-g.y,f.z-g.z )
*(f::Vector3, g::Vector3)=Vector3(f.x*g.x , f.y*g.y,f.z*g.z )
+(f::Vector3, g::Number)=Vector3(f.x+g , f.y+g,f.z+g )
-(f::Vector3, g::Number)=Vector3(f.x-g , f.y-g,f.z-g )
*(f::Vector3, g::Number)=Vector3(f.x*g , f.y*g,f.z*g )
+(g::Vector3, f::Number)=Vector3(f.x+g , f.y+g,f.z+g )
-(g::Vector3, f::Number)=Vector3(g-f.x , g-f.y,g-f.z )
*(g::Vector3, f::Number)=Vector3(f.x*g , f.y*g,f.z*g )
addX(f::Vector3, g::Number)=Vector3(f.x+g , f.y,f.z)
addY(f::Vector3, g::Number)=Vector3(f.x , f.y+g,f.z )
addZ(f::Vector3, g::Number)=Vector3(f.x , f.y,f.z+g )
function normalizeVector3(f::Vector3)
leng=lengthVector3(f)
if(leng>0.0)
return Vector3(f.x/leng,f.y/leng,f.z/leng)
else
return Vector3(0.0,0.0,0.0)
end
end
function lengthVector3(f::Vector3)
leng=sqrt((f.x * f.x) + (f.y * f.y) + (f.z * f.z))
return convert(Float64,leng)
end
function normalizeQuaternion(f::Quaternion)
l = sqrt((f.x * f.x) + (f.y * f.y) + (f.z * f.z)+ (f.w * f.w))
if l === 0
qx = 0
qy = 0
qz = 0
qw = 1
else
l = 1 / l
qx = f.x * l
qy = f.y * l
qz = f.z * l
qw = f.w * l
end
return Quaternion(qx,qy,qz,qw)
end
function normalizeQuaternion1!(fx::Float64,fy::Float64,fz::Float64,fw::Float64)
l = sqrt((fx * fx) + (fy * fy) + (fz * fz)+ (fw * fw))
if l === 0
qx = 0.0
qy = 0.0
qz = 0.0
qw = 1.0
else
l = 1.0 / l
qx = fx * l
qy = fy * l
qz = fz * l
qw = fw * l
end
return qx,qy,qz,qwend
function dotVector3(f::Vector3, g::Vector3)
return (f.x * g.x) + (f.y * g.y) + (f.z * g.z)
end
function Base.show(io::IO, v::Vector3)
print(io, "x:$(v.x), y:$(v.y), z:$(v.z)")
end
function Base.show(io::IO, v::Quaternion)
print(io, "x:$(v.x), y:$(v.y), z:$(v.z), w:$(v.z)")
end
Base.Broadcast.broadcastable(q::Vector3) = Ref(q)
######################utils##################################
function toAxisXVector3(pV::Vector3,axis::Vector3) #TODO CHANGE
xaxis=Vector3(1.0,0.0,0.0)
vector=normalizeVector3(axis)
q=setFromUnitVectors(vector,xaxis)
v=applyQuaternion1( pV ,q )
return Vector3(v.x,v.y,v.z)
end
function toAxisOriginalVector3(pV::Vector3,axis::Vector3)
xaxis=Vector3(1.0,0.0,0.0)
vector=normalizeVector3(axis)
q=setFromUnitVectors(xaxis,vector)
v=applyQuaternion1( pV ,q )
return Vector3(v.x,v.y,v.z)
end
function toAxisXQuat(pQ::Quaternion,axis::Vector3)
xaxis=Vector3(1.0,0.0,0.0)
vector=normalizeVector3(axis)
q=setFromUnitVectors(vector,xaxis)
pV=Vector3(pQ.x,pQ.y,pQ.z)
v=applyQuaternion1( pV ,q )
return Quaternion(v.x,v.y,v.z,pQ.w)
end
function toAxisOriginalQuat(pQ::Vector3,axis::Vector3)
xaxis=Vector3(1.0,0.0,0.0)
vector=normalizeVector3(axis)
q=setFromUnitVectors(xaxis,vector)
pV=Vector3(pQ.x,pQ.y,pQ.z)
v=applyQuaternion1( pV ,q )
return Quaternion(v.x,v.y,v.z,1.0)
end
function roundd(num,prec)
num = convert(Float64,num*prec)
num = floor(num+0.5)
num = convert(Float64,num)
num = num/prec
return num
end
###########################################
function setFromUnitVectors(vFrom::Vector3, vTo::Vector3)
# assumes direction vectors vFrom and vTo are normalized
EPS = 0.000000001;
r= dotVector3(vFrom,vTo)+1.0
# r = dot(vFrom,vTo)+1
if r < EPS
r = 0;
if abs( vFrom.x ) > abs( vFrom.z )
qx = - vFrom.y
qy = vFrom.x
qz = 0.0
qw = r
else
qx = 0.0
qy = -(vFrom.z)
qz = vFrom.y
qw = r
end
else
# crossVectors( vFrom, vTo ); // inlined to avoid cyclic dependency on Vector3
qx = vFrom.y * vTo.z - vFrom.z * vTo.y
qy = vFrom.z * vTo.x - vFrom.x * vTo.z
qz = vFrom.x * vTo.y - vFrom.y * vTo.x
qw = r
end
# qx= (qx==-0.0) ? 0.0 : qx #just commented out 5 august 20202
# qy= (qy==-0.0) ? 0.0 : qy #just commented out 5 august 20202
# qz= (qz==-0.0) ? 0.0 : qz #just commented out 5 august 20202
# qw= (qw==-0.0) ? 0.0 : qw #just commented out 5 august 20202
mx=qx*qx
my=qy*qy
mz=qz*qz
mw=qw*qw
mm=mx+my
mm=mm+mz
mm=mm+mw
mm=convert(Float64,mm)#??????????????????? todo check later
l=CUDAnative.sqrt(mm)
#l = sqrt((qx * qx) + (qy * qy) + (qz * qz)+ (qw * qw))
if l === 0
qx = 0.0
qy = 0.0
qz = 0.0
qw = 1.0
else
l = 1.0 / l
qx = qx * l
qy = qy * l
qz = qz * l
qw = qw * l
end
return Quaternion(qx,qy,qz,qw)
end
function quatToMatrix( quaternion::Quaternion)
#te = RotationMatrix()
x = quaternion.x
y = quaternion.y
z = quaternion.z
w = quaternion.w
x2 = x + x
y2 = y + y
z2 = z + z
xx = x * x2
xy = x * y2
xz = x * z2
yy = y * y2
yz = y * z2
zz = z * z2
wx = w * x2
wy = w * y2
wz = w * z2
sx = 1.0
sy = 1.0
sz = 1.0
te1 = ( 1.0 - ( yy + zz ) ) * sx
te2 = ( xy + wz ) * sx
te3 = ( xz - wy ) * sx
te4 = 0.0
te5 = ( xy - wz ) * sy
te6 = ( 1.0 - ( xx + zz ) ) * sy
te7 = ( yz + wx ) * sy
te8 = 0.0
te9 = ( xz + wy ) * sz
te10 = ( yz - wx ) * sz
te11 = ( 1.0 - ( xx + yy ) ) * sz
te12 = 0.0
te13 = 0.0 #position.x;
te14 = 0.0 #position.y;
te15 = 0.0 #position.z;
te16 = 1.0
te= RotationMatrix(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
return te
end
function setFromRotationMatrix(m::RotationMatrix)
m11 = convert(Float64,m.te1 )
m12 = convert(Float64,m.te5 )
m13 = convert(Float64,m.te9 )
m21 = convert(Float64,m.te2 )
m22 = convert(Float64,m.te6 )
m23 = convert(Float64,m.te10)
m31 = convert(Float64,m.te3 )
m32 = convert(Float64,m.te7 )
m33 = convert(Float64,m.te11)
y = CUDAnative.asin( clamp( m13, -1.0, 1.0 ) ) ##check if has to be changed to cuda
if ( abs( m13 ) < 0.9999999999 )
x = CUDAnative.atan2( - m23, m33 )
z = CUDAnative.atan2( - m12, m11 )#-m12, m11
else
x = CUDAnative.atan2( m32, m22 )
z = 0.0;
end
return Vector3(x,y,z)
end
function setQuaternionFromEuler(euler::Vector3)
x=euler.x
y=euler.y
z=euler.z
c1 = CUDAnative.cos( x / 2.0 )
c2 = CUDAnative.cos( y / 2.0 )
c3 = CUDAnative.cos( z / 2.0 )
s1 = CUDAnative.sin( x / 2.0 )
s2 = CUDAnative.sin( y / 2.0 )
s3 = CUDAnative.sin( z / 2.0 )
x = s1 * c2 * c3 + c1 * s2 * s3
y = c1 * s2 * c3 - s1 * c2 * s3
z = c1 * c2 * s3 + s1 * s2 * c3
w = c1 * c2 * c3 - s1 * s2 * s3
return Quaternion(x,y,z,w)
end
function applyQuaternion1(e::Vector3,q2::Quaternion)
x = e.x
y = e.y
z = e.z
qx = q2.x
qy = q2.y
qz = q2.z
qw = q2.w
# calculate quat * vector
ix = qw * x + qy * z - qz * y
iy = qw * y + qz * x - qx * z
iz = qw * z + qx * y - qy * x
iw = - qx * x - qy * y - qz * z
# calculate result * inverse quat
xx = ix * qw + iw * - qx + iy * - qz - iz * - qy
yy = iy * qw + iw * - qy + iz * - qx - ix * - qz
zz = iz * qw + iw * - qz + ix * - qy - iy * - qx
d=15
return Vector3(xx,yy,zz)
end
##########################################
function conjugate(q::Quaternion)
x= (-q.x==-0) ? 0.0 : -q.x
y= (-q.y==-0) ? 0.0 : -q.y
z= (-q.z==-0) ? 0.0 : -q.z
w=q.w x=-q.x
y=-q.y
z=-q.z
x=convert(Float64,x)
y=convert(Float64,y)
z=convert(Float64,z)
w=convert(Float64,w)
return Quaternion(x,y,z,w)
end
function RotateVec3D(a::Quaternion, f::Vector3)
fx= (f.x==-0) ? 0 : f.x
fy= (f.y==-0) ? 0 : f.y
fz= (f.z==-0) ? 0 : f.z
# fx= f.x
# fy= f.y
# fz= f.z
tw = fx*a.x + fy*a.y + fz*a.z
tx = fx*a.w - fy*a.z + fz*a.y
ty = fx*a.z + fy*a.w - fz*a.x
tz = -fx*a.y + fy*a.x + fz*a.w
return Vector3((a.w*tx+a.x*tw+a.y*tz-a.z*ty),(a.w*ty-a.x*tz+a.y*tw+a.z*tx),(a.w*tz+a.x*ty-a.y*tx+a.z*tw))
end
#!< Returns a vector representing the specified vector "f" rotated by this quaternion. @param[in] f The vector to transform.
function RotateVec3DInv(a::Quaternion, f::Vector3)
fx=f.x
fy=f.y
fz=f.z
tw = a.x*fx + a.y*fy + a.z*fz
tx = a.w*fx - a.y*fz + a.z*fy
ty = a.w*fy + a.x*fz - a.z*fx
tz = a.w*fz - a.x*fy + a.y*fx
return Vector3((tw*a.x + tx*a.w + ty*a.z - tz*a.y),(tw*a.y - tx*a.z + ty*a.w + tz*a.x),(tw*a.z + tx*a.y - ty*a.x + tz*a.w))
end
#!< Returns a vector representing the specified vector "f" rotated by the inverse of this quaternion. This is the opposite of RotateVec3D. @param[in] f The vector to transform.
function ToRotationVector(a::Quaternion)
x=convert(Float64,a.x)
y=convert(Float64,a.y)
z=convert(Float64,a.z)
w=convert(Float64,a.w)
if (w >= 1.0 || w <= -1.0)
return Vector3(0.0,0.0,0.0)
end
squareLength = 1.0-w*w; # because x*x + y*y + z*z + w*w = 1.0, but more susceptible to w noise (when
SLTHRESH_ACOS2SQRT= 2.4e-3; # SquareLength threshhold for when we can use square root optimization for acos. From SquareLength = 1-w*w. (calculate according to 1.0-W_THRESH_ACOS2SQRT*W_THRESH_ACOS2SQRT
if (squareLength < SLTHRESH_ACOS2SQRT) # ???????
# if (squareLength==0.0)
# w=1.0-1e-12
# squareLength = 1.0-w*w
# end
xx=x*(2.0*CUDAnative.sqrt((2.0-2.0*w)/squareLength))
yy=y*(2.0*CUDAnative.sqrt((2.0-2.0*w)/squareLength))
zz=z*(2.0*CUDAnative.sqrt((2.0-2.0*w)/squareLength))
xx=convert(Float64,xx)
yy=convert(Float64,yy)
zz=convert(Float64,zz)
return Vector3(xx,yy,zz) ; # acos(w) = sqrt(2*(1-x)) for w close to 1. for w=0.001, error is 1.317e-6
else
xx=x*(2.0*CUDAnative.acos(w)/CUDAnative.sqrt(squareLength))
yy=y*(2.0*CUDAnative.acos(w)/CUDAnative.sqrt(squareLength))
zz=z*(2.0*CUDAnative.acos(w)/CUDAnative.sqrt(squareLength))
xx=convert(Float64,xx)
yy=convert(Float64,yy)
zz=convert(Float64,zz)
return Vector3(xx,yy,zz)
end
# if (a.w >= 1.0 || a.w <= -1.0)
# return Vector3(0.0,0.0,0.0)
# end
# squareLength = 1.0-a.w*a.w; #because x*x + y*y + z*z + w*w = 1.0, but more susceptible to w noise (when
# if (squareLength < SLTHRESH_ACOS2SQRT)
# return Vec3D<T>(x, y, z)*2.0*sqrt((2-2*a.w)/squareLength); #acos(w) = sqrt(2*(1-x)) for w close to 1. for w=0.001, error is 1.317e-6
# else
# return Vec3D<T>(x, y, z)*2.0*acos(a.w)/sqrt(squareLength);
# end
end
# !< Returns a rotation vector representing this quaternion rotation. Adapted from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/
function FromRotationVector(VecIn::Vector3)
theta=VecIn*Vector3(0.5,0.5,0.5)
ntheta=CUDAnative.sqrt((theta.x * theta.x) + (theta.y * theta.y) + (theta.z * theta.z))
thetaMag2=ntheta*ntheta
DBL_EPSILONx24 =5.328e-15
if thetaMag2*thetaMag2 < DBL_EPSILONx24
qw=1.0 - 0.5*thetaMag2
s=1.0 - thetaMag2 / 6.0
else
thetaMag = CUDAnative.sqrt(thetaMag2)
qw=CUDAnative.cos(thetaMag)
s=CUDAnative.sin(thetaMag) / thetaMag
end
qx=theta.x*s
qy=theta.y*s
qz=theta.z*s
qx=convert(Float64,qx)
qy=convert(Float64,qy)
qz=convert(Float64,qz)
qw=convert(Float64,qw)
return Quaternion(qx,qy,qz,qw)
end
function multiplyQuaternions(q::Quaternion,f::Quaternion)
x=convert(Float64,q.x)
y=convert(Float64,q.y)
z=convert(Float64,q.z)
w=convert(Float64,q.w)
x1=convert(Float64,w*convert(Float64,f.x) + x*convert(Float64,f.w) + y*convert(Float64,f.z) - z*convert(Float64,f.y))
y1=convert(Float64,w*convert(Float64,f.y) - x*convert(Float64,f.z) + y*convert(Float64,f.w) + z*convert(Float64,f.x))
z1=convert(Float64,w*convert(Float64,f.z) + x*convert(Float64,f.y) - y*convert(Float64,f.x) + z*convert(Float64,f.w))
w1=convert(Float64,w*convert(Float64,f.w) - x*convert(Float64,f.x) - y*convert(Float64,f.y) - z*convert(Float64,f.z))
# w1=convert(Float64,1.0*convert(Float64,f.w) - 1.0*convert(Float64,f.x) + 1.0*convert(Float64,f.y) - 1.0*convert(Float64,f.z)) #todo wrong fix
return Quaternion(x1,y1,z1,w1 ); #!< overload quaternion multiplication.
end
#####################################################
function FromAngleToPosX(angle::Quaternion,RotateFrom::Vector3) #highly optimized at the expense of readability
if (RotateFrom.x==0.0 && RotateFrom.y==0.0 && RotateFrom.z==0.0)
return Quaternion(angle.x,angle.y,angle.z,angle.w ) #leave off if it slows down too much!!
end
#Catch and handle small angle:
YoverX = RotateFrom.y/RotateFrom.x;
ZoverX = RotateFrom.z/RotateFrom.x;
SMALL_ANGLE_RAD= 1.732e-2 #/Angles less than this get small angle approximations. To get: Root solve atan(t)/t-1+MAX_ERROR_PERCENT. From: MAX_ERROR_PERCENT = (t-atan(t))/t
SMALL_ANGLE_W= 0.9999625 #//quaternion W value corresponding to a SMALL_ANGLE_RAD. To calculate, cos(SMALL_ANGLE_RAD*0.5).
PI=3.14159265358979
DISCARD_ANGLE_RAD=1e-7#define DISCARD_ANGLE_RAD 1e-7 //Anything less than this angle can be considered 0
if (YoverX<SMALL_ANGLE_RAD && YoverX>-SMALL_ANGLE_RAD && ZoverX<SMALL_ANGLE_RAD && ZoverX>-SMALL_ANGLE_RAD)#Intercept small angle and zero angle x=0.0
y=0.5*ZoverX
z=-0.5*YoverX
w = 1.0+0.5*(-y*y-z*z) #w=sqrt(1-x*x-y*y), small angle sqrt(1+x) ~= 1+x/2 at x near zero.
return Quaternion(x,y,z,w )
end
#more accurate non-small angle:
RotFromNorm = Vector3(RotateFrom.x,RotateFrom.y,RotateFrom.z);
RotFromNorm=normalizeVector3(RotFromNorm); #Normalize the input...
theta = CUDAnative.acos(RotFromNorm.x); #because RotFromNorm is normalized, 1,0,0 is normalized, and A.B = |A||B|cos(theta) = cos(theta)
if (theta > PI-DISCARD_ANGLE_RAD)
w=0.0
x=0.0
y=1.0
z=0.0;
return Quaternion(x,y,z,w )
end #180 degree rotation (about y axis, since the vector must be pointing in -x direction
AxisMagInv = 1.0/CUDAnative.sqrt(RotFromNorm.z*RotFromNorm.z+RotFromNorm.y*RotFromNorm.y);
#Here theta is the angle, axis is RotFromNorm.Cross(Vec3D(1,0,0)) = Vec3D(0, RotFromNorm.z/AxisMagInv, -RotFromNorm.y/AxisMagInv), which is still normalized. (super rolled together)
a = 0.5*theta;
s = CUDAnative.sin(a);
w=CUDAnative.cos(a);
x=0;
y=RotFromNorm.z*AxisMagInv*s;
z = -RotFromNorm.y*AxisMagInv*s; #angle axis function, reduced
return Quaternion(x,y,z,w )
end
# //!< Overwrites this quaternion with the calculated rotation that would transform the specified RotateFrom vector to point in the positve X direction. Note: function changes this quaternion. @param[in] RotateFrom An arbitrary direction vector. Does not need to be normalized.