diff options
| -rw-r--r-- | changelog.md | 4 | ||||
| -rw-r--r-- | lib/pure/basic2d.nim | 855 | ||||
| -rw-r--r-- | lib/pure/basic3d.nim | 1040 |
3 files changed, 4 insertions, 1895 deletions
diff --git a/changelog.md b/changelog.md index 3c0048199..9f6a83a37 100644 --- a/changelog.md +++ b/changelog.md @@ -1,3 +1,7 @@ ## v0.18.0 - dd/mm/yyyy +### Changes affecting backwards compatibility +- Removed basic2d/basic3d out of the stdlib and into Nimble packages. + These packages deprecated however, use the ``glm``, ``arraymancer``, ``neo`` + or another package. diff --git a/lib/pure/basic2d.nim b/lib/pure/basic2d.nim deleted file mode 100644 index 31b3814d6..000000000 --- a/lib/pure/basic2d.nim +++ /dev/null @@ -1,855 +0,0 @@ -# -# -# Nim's Runtime Library -# (c) Copyright 2013 Robert Persson -# -# See the file "copying.txt", included in this -# distribution, for details about the copyright. -# - -import math -import strutils - - -## Basic 2d support with vectors, points, matrices and some basic utilities. -## Vectors are implemented as direction vectors, ie. when transformed with a matrix -## the translation part of matrix is ignored. -## Operators `+` , `-` , `*` , `/` , `+=` , `-=` , `*=` and `/=` are implemented for vectors and scalars. -## -## Quick start example: -## -## .. code-block:: nim -## -## # Create a matrix which first rotates, then scales and at last translates -## -## var m:Matrix2d=rotate(DEG90) & scale(2.0) & move(100.0,200.0) -## -## # Create a 2d point at (100,0) and a vector (5,2) -## -## var pt:Point2d=point2d(100.0,0.0) -## -## var vec:Vector2d=vector2d(5.0,2.0) -## -## -## pt &= m # transforms pt in place -## -## var pt2:Point2d=pt & m #concatenates pt with m and returns a new point -## -## var vec2:Vector2d=vec & m #concatenates vec with m and returns a new vector - - -const - DEG360* = PI * 2.0 - ## 360 degrees in radians. - DEG270* = PI * 1.5 - ## 270 degrees in radians. - DEG180* = PI - ## 180 degrees in radians. - DEG90* = PI / 2.0 - ## 90 degrees in radians. - DEG60* = PI / 3.0 - ## 60 degrees in radians. - DEG45* = PI / 4.0 - ## 45 degrees in radians. - DEG30* = PI / 6.0 - ## 30 degrees in radians. - DEG15* = PI / 12.0 - ## 15 degrees in radians. - RAD2DEGCONST = 180.0 / PI - ## used internally by DegToRad and RadToDeg - -type - Matrix2d* = object - ## Implements a row major 2d matrix, which means - ## transformations are applied the order they are concatenated. - ## The rightmost column of the 3x3 matrix is left out since normally - ## not used for geometric transformations in 2d. - ax*,ay*,bx*,by*,tx*,ty*:float - Point2d* = object - ## Implements a non-homogeneous 2d point stored as - ## an `x` coordinate and an `y` coordinate. - x*,y*:float - Vector2d* = object - ## Implements a 2d **direction vector** stored as - ## an `x` coordinate and an `y` coordinate. Direction vector means, - ## that when transforming a vector with a matrix, the translational - ## part of the matrix is ignored. - x*,y*:float -{.deprecated: [TMatrix2d: Matrix2d, TPoint2d: Point2d, TVector2d: Vector2d].} - - -# Some forward declarations... -proc matrix2d*(ax,ay,bx,by,tx,ty:float):Matrix2d {.noInit.} - ## Creates a new matrix. - ## `ax`,`ay` is the local x axis - ## `bx`,`by` is the local y axis - ## `tx`,`ty` is the translation -proc vector2d*(x,y:float):Vector2d {.noInit,inline.} - ## Returns a new vector (`x`,`y`) -proc point2d*(x,y:float):Point2d {.noInit,inline.} - ## Returns a new point (`x`,`y`) - - - -let - IDMATRIX*:Matrix2d=matrix2d(1.0,0.0,0.0,1.0,0.0,0.0) - ## Quick access to an identity matrix - ORIGO*:Point2d=point2d(0.0,0.0) - ## Quick access to point (0,0) - XAXIS*:Vector2d=vector2d(1.0,0.0) - ## Quick access to an 2d x-axis unit vector - YAXIS*:Vector2d=vector2d(0.0,1.0) - ## Quick access to an 2d y-axis unit vector - - -# *************************************** -# Private utils -# *************************************** - -proc rtos(val:float):string= - return formatFloat(val,ffDefault,0) - -proc safeArccos(v:float):float= - ## assumes v is in range 0.0-1.0, but clamps - ## the value to avoid out of domain errors - ## due to rounding issues - return arccos(clamp(v,-1.0,1.0)) - - -template makeBinOpVector(s) = - ## implements binary operators ``+``, ``-``, ``*`` and ``/`` for vectors - proc s*(a,b:Vector2d):Vector2d {.inline,noInit.} = vector2d(s(a.x,b.x),s(a.y,b.y)) - proc s*(a:Vector2d,b:float):Vector2d {.inline,noInit.} = vector2d(s(a.x,b),s(a.y,b)) - proc s*(a:float,b:Vector2d):Vector2d {.inline,noInit.} = vector2d(s(a,b.x),s(a,b.y)) - -template makeBinOpAssignVector(s)= - ## implements inplace binary operators ``+=``, ``-=``, ``/=`` and ``*=`` for vectors - proc s*(a:var Vector2d,b:Vector2d) {.inline.} = s(a.x,b.x) ; s(a.y,b.y) - proc s*(a:var Vector2d,b:float) {.inline.} = s(a.x,b) ; s(a.y,b) - - -# *************************************** -# Matrix2d implementation -# *************************************** - -proc setElements*(t:var Matrix2d,ax,ay,bx,by,tx,ty:float) {.inline.}= - ## Sets arbitrary elements in an existing matrix. - t.ax=ax - t.ay=ay - t.bx=bx - t.by=by - t.tx=tx - t.ty=ty - -proc matrix2d*(ax,ay,bx,by,tx,ty:float):Matrix2d = - result.setElements(ax,ay,bx,by,tx,ty) - -proc `&`*(a,b:Matrix2d):Matrix2d {.noInit.} = #concatenate matrices - ## Concatenates matrices returning a new matrix. - - # | a.AX a.AY 0 | | b.AX b.AY 0 | - # | a.BX a.BY 0 | * | b.BX b.BY 0 | - # | a.TX a.TY 1 | | b.TX b.TY 1 | - result.setElements( - a.ax * b.ax + a.ay * b.bx, - a.ax * b.ay + a.ay * b.by, - a.bx * b.ax + a.by * b.bx, - a.bx * b.ay + a.by * b.by, - a.tx * b.ax + a.ty * b.bx + b.tx, - a.tx * b.ay + a.ty * b.by + b.ty) - - -proc scale*(s:float):Matrix2d {.noInit.} = - ## Returns a new scale matrix. - result.setElements(s,0,0,s,0,0) - -proc scale*(s:float,org:Point2d):Matrix2d {.noInit.} = - ## Returns a new scale matrix using, `org` as scale origin. - result.setElements(s,0,0,s,org.x-s*org.x,org.y-s*org.y) - -proc stretch*(sx,sy:float):Matrix2d {.noInit.} = - ## Returns new a stretch matrix, which is a - ## scale matrix with non uniform scale in x and y. - result.setElements(sx,0,0,sy,0,0) - -proc stretch*(sx,sy:float,org:Point2d):Matrix2d {.noInit.} = - ## Returns a new stretch matrix, which is a - ## scale matrix with non uniform scale in x and y. - ## `org` is used as stretch origin. - result.setElements(sx,0,0,sy,org.x-sx*org.x,org.y-sy*org.y) - -proc move*(dx,dy:float):Matrix2d {.noInit.} = - ## Returns a new translation matrix. - result.setElements(1,0,0,1,dx,dy) - -proc move*(v:Vector2d):Matrix2d {.noInit.} = - ## Returns a new translation matrix from a vector. - result.setElements(1,0,0,1,v.x,v.y) - -proc rotate*(rad:float):Matrix2d {.noInit.} = - ## Returns a new rotation matrix, which - ## represents a rotation by `rad` radians - let - s=sin(rad) - c=cos(rad) - result.setElements(c,s,-s,c,0,0) - -proc rotate*(rad:float,org:Point2d):Matrix2d {.noInit.} = - ## Returns a new rotation matrix, which - ## represents a rotation by `rad` radians around - ## the origin `org` - let - s=sin(rad) - c=cos(rad) - result.setElements(c,s,-s,c,org.x+s*org.y-c*org.x,org.y-c*org.y-s*org.x) - -proc mirror*(v:Vector2d):Matrix2d {.noInit.} = - ## Returns a new mirror matrix, mirroring - ## around the line that passes through origo and - ## has the direction of `v` - let - sqx=v.x*v.x - sqy=v.y*v.y - nd=1.0/(sqx+sqy) #used to normalize invector - xy2=v.x*v.y*2.0*nd - sqd=nd*(sqx-sqy) - - if nd==Inf or nd==NegInf: - return IDMATRIX #mirroring around a zero vector is arbitrary=>just use identity - - result.setElements( - sqd,xy2, - xy2,-sqd, - 0.0,0.0) - -proc mirror*(org:Point2d,v:Vector2d):Matrix2d {.noInit.} = - ## Returns a new mirror matrix, mirroring - ## around the line that passes through `org` and - ## has the direction of `v` - let - sqx=v.x*v.x - sqy=v.y*v.y - nd=1.0/(sqx+sqy) #used to normalize invector - xy2=v.x*v.y*2.0*nd - sqd=nd*(sqx-sqy) - - if nd==Inf or nd==NegInf: - return IDMATRIX #mirroring around a zero vector is arbitrary=>just use identity - - result.setElements( - sqd,xy2, - xy2,-sqd, - org.x-org.y*xy2-org.x*sqd,org.y-org.x*xy2+org.y*sqd) - - - -proc skew*(xskew,yskew:float):Matrix2d {.noInit.} = - ## Returns a new skew matrix, which has its - ## x axis rotated `xskew` radians from the local x axis, and - ## y axis rotated `yskew` radians from the local y axis - result.setElements(cos(yskew),sin(yskew),-sin(xskew),cos(xskew),0,0) - - -proc `$`* (t:Matrix2d):string {.noInit.} = - ## Returns a string representation of the matrix - return rtos(t.ax) & "," & rtos(t.ay) & - "," & rtos(t.bx) & "," & rtos(t.by) & - "," & rtos(t.tx) & "," & rtos(t.ty) - -proc isUniform*(t:Matrix2d,tol=1.0e-6):bool= - ## Checks if the transform is uniform, that is - ## perpendicular axes of equal length, which means (for example) - ## it cannot transform a circle into an ellipse. - ## `tol` is used as tolerance for both equal length comparison - ## and perp. comparison. - - #dot product=0 means perpendicular coord. system: - if abs(t.ax*t.bx+t.ay*t.by)<=tol: - #subtract squared lengths of axes to check if uniform scaling: - if abs((t.ax*t.ax+t.ay*t.ay)-(t.bx*t.bx+t.by*t.by))<=tol: - return true - return false - -proc determinant*(t:Matrix2d):float= - ## Computes the determinant of the matrix. - - #NOTE: equivalent with perp.dot product for two 2d vectors - return t.ax*t.by-t.bx*t.ay - -proc isMirroring* (m:Matrix2d):bool= - ## Checks if the `m` is a mirroring matrix, - ## which means it will reverse direction of a curve transformed with it - return m.determinant<0.0 - -proc inverse*(m:Matrix2d):Matrix2d {.noInit.} = - ## Returns a new matrix, which is the inverse of the matrix - ## If the matrix is not invertible (determinant=0), an EDivByZero - ## will be raised. - let d=m.determinant - if d==0.0: - raise newException(DivByZeroError,"Cannot invert a zero determinant matrix") - - result.setElements( - m.by/d,-m.ay/d, - -m.bx/d,m.ax/d, - (m.bx*m.ty-m.by*m.tx)/d, - (m.ay*m.tx-m.ax*m.ty)/d) - -proc equals*(m1:Matrix2d,m2:Matrix2d,tol=1.0e-6):bool= - ## Checks if all elements of `m1`and `m2` is equal within - ## a given tolerance `tol`. - return - abs(m1.ax-m2.ax)<=tol and - abs(m1.ay-m2.ay)<=tol and - abs(m1.bx-m2.bx)<=tol and - abs(m1.by-m2.by)<=tol and - abs(m1.tx-m2.tx)<=tol and - abs(m1.ty-m2.ty)<=tol - -proc `=~`*(m1,m2:Matrix2d):bool= - ## Checks if `m1`and `m2` is approximately equal, using a - ## tolerance of 1e-6. - equals(m1,m2) - -proc isIdentity*(m:Matrix2d,tol=1.0e-6):bool= - ## Checks is a matrix is approximately an identity matrix, - ## using `tol` as tolerance for each element. - return equals(m,IDMATRIX,tol) - -proc apply*(m:Matrix2d,x,y:var float,translate=false)= - ## Applies transformation `m` onto `x`,`y`, optionally - ## using the translation part of the matrix. - if translate: # positional style transform - let newx=x*m.ax+y*m.bx+m.tx - y=x*m.ay+y*m.by+m.ty - x=newx - else: # delta style transform - let newx=x*m.ax+y*m.bx - y=x*m.ay+y*m.by - x=newx - - - -# *************************************** -# Vector2d implementation -# *************************************** -proc vector2d*(x,y:float):Vector2d = #forward decl. - result.x=x - result.y=y - -proc polarVector2d*(ang:float,len:float):Vector2d {.noInit.} = - ## Returns a new vector with angle `ang` and magnitude `len` - result.x=cos(ang)*len - result.y=sin(ang)*len - -proc slopeVector2d*(slope:float,len:float):Vector2d {.noInit.} = - ## Returns a new vector having slope (dy/dx) given by - ## `slope`, and a magnitude of `len` - let ang=arctan(slope) - result.x=cos(ang)*len - result.y=sin(ang)*len - -proc len*(v:Vector2d):float {.inline.}= - ## Returns the length of the vector. - sqrt(v.x*v.x+v.y*v.y) - -proc `len=`*(v:var Vector2d,newlen:float) {.noInit.} = - ## Sets the length of the vector, keeping its angle. - let fac=newlen/v.len - - if newlen==0.0: - v.x=0.0 - v.y=0.0 - return - - if fac==Inf or fac==NegInf: - #to short for float accuracy - #do as good as possible: - v.x=newlen - v.y=0.0 - else: - v.x*=fac - v.y*=fac - -proc sqrLen*(v:Vector2d):float {.inline.}= - ## Computes the squared length of the vector, which is - ## faster than computing the absolute length. - v.x*v.x+v.y*v.y - -proc angle*(v:Vector2d):float= - ## Returns the angle of the vector. - ## (The counter clockwise plane angle between posetive x axis and `v`) - result=arctan2(v.y,v.x) - if result<0.0: result+=DEG360 - -proc `$` *(v:Vector2d):string= - ## String representation of `v` - result=rtos(v.x) - result.add(",") - result.add(rtos(v.y)) - - -proc `&` *(v:Vector2d,m:Matrix2d):Vector2d {.noInit.} = - ## Concatenate vector `v` with a transformation matrix. - ## Transforming a vector ignores the translational part - ## of the matrix. - - # | AX AY 0 | - # | X Y 1 | * | BX BY 0 | - # | 0 0 1 | - result.x=v.x*m.ax+v.y*m.bx - result.y=v.x*m.ay+v.y*m.by - - -proc `&=`*(v:var Vector2d,m:Matrix2d) {.inline.}= - ## Applies transformation `m` onto `v` in place. - ## Transforming a vector ignores the translational part - ## of the matrix. - - # | AX AY 0 | - # | X Y 1 | * | BX BY 0 | - # | 0 0 1 | - let newx=v.x*m.ax+v.y*m.bx - v.y=v.x*m.ay+v.y*m.by - v.x=newx - - -proc tryNormalize*(v:var Vector2d):bool= - ## Modifies `v` to have a length of 1.0, keeping its angle. - ## If `v` has zero length (and thus no angle), it is left unmodified and - ## false is returned, otherwise true is returned. - - let mag=v.len - - if mag==0.0: - return false - - v.x/=mag - v.y/=mag - return true - - -proc normalize*(v:var Vector2d) {.inline.}= - ## Modifies `v` to have a length of 1.0, keeping its angle. - ## If `v` has zero length, an EDivByZero will be raised. - if not tryNormalize(v): - raise newException(DivByZeroError,"Cannot normalize zero length vector") - -proc transformNorm*(v:var Vector2d,t:Matrix2d)= - ## Applies a normal direction transformation `t` onto `v` in place. - ## The resulting vector is *not* normalized. Transforming a vector ignores the - ## translational part of the matrix. If the matrix is not invertible - ## (determinant=0), an EDivByZero will be raised. - - # transforming a normal is done by transforming - # by the transpose of the inverse of the original matrix - # this can be heavily optimized by precompute and inline - # | | AX AY 0 | ^-1| ^T - # | X Y 1 | * | | BX BY 0 | | - # | | 0 0 1 | | - let d=t.determinant - if(d==0.0): - raise newException(DivByZeroError,"Matrix is not invertible") - let newx = (t.by*v.x-t.ay*v.y)/d - v.y = (t.ax*v.y-t.bx*v.x)/d - v.x = newx - -proc transformInv*(v:var Vector2d,t:Matrix2d)= - ## Applies inverse of a transformation `t` to `v` in place. - ## This is faster than creating an inverse matrix and apply() it. - ## Transforming a vector ignores the translational part - ## of the matrix. If the matrix is not invertible (determinant=0), an EDivByZero - ## will be raised. - let d=t.determinant - - if(d==0.0): - raise newException(DivByZeroError,"Matrix is not invertible") - - let newx=(t.by*v.x-t.bx*v.y)/d - v.y = (t.ax*v.y-t.ay*v.x)/d - v.x = newx - -proc transformNormInv*(v:var Vector2d,t:Matrix2d)= - ## Applies an inverse normal direction transformation `t` onto `v` in place. - ## This is faster than creating an inverse - ## matrix and transformNorm(...) it. Transforming a vector ignores the - ## translational part of the matrix. - - # normal inverse transform is done by transforming - # by the inverse of the transpose of the inverse of the org. matrix - # which is equivalent with transforming with the transpose. - # | | | AX AY 0 |^-1|^T|^-1 | AX BX 0 | - # | X Y 1 | * | | | BX BY 0 | | | = | X Y 1 | * | AY BY 0 | - # | | | 0 0 1 | | | | 0 0 1 | - # This can be heavily reduced to: - let newx=t.ay*v.y+t.ax*v.x - v.y=t.by*v.y+t.bx*v.x - v.x=newx - -proc rotate90*(v:var Vector2d) {.inline.}= - ## Quickly rotates vector `v` 90 degrees counter clockwise, - ## without using any trigonometrics. - swap(v.x,v.y) - v.x= -v.x - -proc rotate180*(v:var Vector2d){.inline.}= - ## Quickly rotates vector `v` 180 degrees counter clockwise, - ## without using any trigonometrics. - v.x= -v.x - v.y= -v.y - -proc rotate270*(v:var Vector2d) {.inline.}= - ## Quickly rotates vector `v` 270 degrees counter clockwise, - ## without using any trigonometrics. - swap(v.x,v.y) - v.y= -v.y - -proc rotate*(v:var Vector2d,rad:float) = - ## Rotates vector `v` `rad` radians in place. - let - s=sin(rad) - c=cos(rad) - newx=c*v.x-s*v.y - v.y=c*v.y+s*v.x - v.x=newx - -proc scale*(v:var Vector2d,fac:float){.inline.}= - ## Scales vector `v` `rad` radians in place. - v.x*=fac - v.y*=fac - -proc stretch*(v:var Vector2d,facx,facy:float){.inline.}= - ## Stretches vector `v` `facx` times horizontally, - ## and `facy` times vertically. - v.x*=facx - v.y*=facy - -proc mirror*(v:var Vector2d,mirrvec:Vector2d)= - ## Mirrors vector `v` using `mirrvec` as mirror direction. - let - sqx=mirrvec.x*mirrvec.x - sqy=mirrvec.y*mirrvec.y - nd=1.0/(sqx+sqy) #used to normalize invector - xy2=mirrvec.x*mirrvec.y*2.0*nd - sqd=nd*(sqx-sqy) - - if nd==Inf or nd==NegInf: - return #mirroring around a zero vector is arbitrary=>keep as is is fastest - - let newx=xy2*v.y+sqd*v.x - v.y=v.x*xy2-sqd*v.y - v.x=newx - - -proc `-` *(v:Vector2d):Vector2d= - ## Negates a vector - result.x= -v.x - result.y= -v.y - -# declare templated binary operators -makeBinOpVector(`+`) -makeBinOpVector(`-`) -makeBinOpVector(`*`) -makeBinOpVector(`/`) -makeBinOpAssignVector(`+=`) -makeBinOpAssignVector(`-=`) -makeBinOpAssignVector(`*=`) -makeBinOpAssignVector(`/=`) - - -proc dot*(v1,v2:Vector2d):float= - ## Computes the dot product of two vectors. - ## Returns 0.0 if the vectors are perpendicular. - return v1.x*v2.x+v1.y*v2.y - -proc cross*(v1,v2:Vector2d):float= - ## Computes the cross product of two vectors, also called - ## the 'perpendicular dot product' in 2d. Returns 0.0 if the vectors - ## are parallel. - return v1.x*v2.y-v1.y*v2.x - -proc equals*(v1,v2:Vector2d,tol=1.0e-6):bool= - ## Checks if two vectors approximately equals with a tolerance. - return abs(v2.x-v1.x)<=tol and abs(v2.y-v1.y)<=tol - -proc `=~` *(v1,v2:Vector2d):bool= - ## Checks if two vectors approximately equals with a - ## hardcoded tolerance 1e-6 - equals(v1,v2) - -proc angleTo*(v1,v2:Vector2d):float= - ## Returns the smallest of the two possible angles - ## between `v1` and `v2` in radians. - var - nv1=v1 - nv2=v2 - if not nv1.tryNormalize or not nv2.tryNormalize: - return 0.0 # zero length vector has zero angle to any other vector - return safeArccos(dot(nv1,nv2)) - -proc angleCCW*(v1,v2:Vector2d):float= - ## Returns the counter clockwise plane angle from `v1` to `v2`, - ## in range 0 - 2*PI - let a=v1.angleTo(v2) - if v1.cross(v2)>=0.0: - return a - return DEG360-a - -proc angleCW*(v1,v2:Vector2d):float= - ## Returns the clockwise plane angle from `v1` to `v2`, - ## in range 0 - 2*PI - let a=v1.angleTo(v2) - if v1.cross(v2)<=0.0: - return a - return DEG360-a - -proc turnAngle*(v1,v2:Vector2d):float= - ## Returns the amount v1 should be rotated (in radians) to equal v2, - ## in range -PI to PI - let a=v1.angleTo(v2) - if v1.cross(v2)<=0.0: - return -a - return a - -proc bisect*(v1,v2:Vector2d):Vector2d {.noInit.}= - ## Computes the bisector between v1 and v2 as a normalized vector. - ## If one of the input vectors has zero length, a normalized version - ## of the other is returned. If both input vectors has zero length, - ## an arbitrary normalized vector is returned. - var - vmag1=v1.len - vmag2=v2.len - - # zero length vector equals arbitrary vector, just change to magnitude to one to - # avoid zero division - if vmag1==0.0: - if vmag2==0: #both are zero length return any normalized vector - return XAXIS - vmag1=1.0 - if vmag2==0.0: vmag2=1.0 - - let - x1=v1.x/vmag1 - y1=v1.y/vmag1 - x2=v2.x/vmag2 - y2=v2.y/vmag2 - - result.x=(x1 + x2) * 0.5 - result.y=(y1 + y2) * 0.5 - - if not result.tryNormalize(): - # This can happen if vectors are colinear. In this special case - # there are actually two bisectors, we select just - # one of them (x1,y1 rotated 90 degrees ccw). - result.x = -y1 - result.y = x1 - - - -# *************************************** -# Point2d implementation -# *************************************** - -proc point2d*(x,y:float):Point2d = - result.x=x - result.y=y - -proc sqrDist*(a,b:Point2d):float= - ## Computes the squared distance between `a` and `b` - let dx=b.x-a.x - let dy=b.y-a.y - result=dx*dx+dy*dy - -proc dist*(a,b:Point2d):float {.inline.}= - ## Computes the absolute distance between `a` and `b` - result=sqrt(sqrDist(a,b)) - -proc angle*(a,b:Point2d):float= - ## Computes the angle of the vector `b`-`a` - let dx=b.x-a.x - let dy=b.y-a.y - result=arctan2(dy,dx) - if result<0: - result += DEG360 - -proc `$` *(p:Point2d):string= - ## String representation of `p` - result=rtos(p.x) - result.add(",") - result.add(rtos(p.y)) - -proc `&`*(p:Point2d,t:Matrix2d):Point2d {.noInit,inline.} = - ## Concatenates a point `p` with a transform `t`, - ## resulting in a new, transformed point. - - # | AX AY 0 | - # | X Y 1 | * | BX BY 0 | - # | TX TY 1 | - result.x=p.x*t.ax+p.y*t.bx+t.tx - result.y=p.x*t.ay+p.y*t.by+t.ty - -proc `&=` *(p:var Point2d,t:Matrix2d) {.inline.}= - ## Applies transformation `t` onto `p` in place. - let newx=p.x*t.ax+p.y*t.bx+t.tx - p.y=p.x*t.ay+p.y*t.by+t.ty - p.x=newx - - -proc transformInv*(p:var Point2d,t:Matrix2d){.inline.}= - ## Applies the inverse of transformation `t` onto `p` in place. - ## If the matrix is not invertable (determinant=0) , EDivByZero will - ## be raised. - - # | AX AY 0 | ^-1 - # | X Y 1 | * | BX BY 0 | - # | TX TY 1 | - let d=t.determinant - if d==0.0: - raise newException(DivByZeroError,"Cannot invert a zero determinant matrix") - let - newx= (t.bx*t.ty-t.by*t.tx+p.x*t.by-p.y*t.bx)/d - p.y = -(t.ax*t.ty-t.ay*t.tx+p.x*t.ay-p.y*t.ax)/d - p.x=newx - - -proc `+`*(p:Point2d,v:Vector2d):Point2d {.noInit,inline.} = - ## Adds a vector `v` to a point `p`, resulting - ## in a new point. - result.x=p.x+v.x - result.y=p.y+v.y - -proc `+=`*(p:var Point2d,v:Vector2d) {.noInit,inline.} = - ## Adds a vector `v` to a point `p` in place. - p.x+=v.x - p.y+=v.y - -proc `-`*(p:Point2d,v:Vector2d):Point2d {.noInit,inline.} = - ## Subtracts a vector `v` from a point `p`, resulting - ## in a new point. - result.x=p.x-v.x - result.y=p.y-v.y - -proc `-`*(p1,p2:Point2d):Vector2d {.noInit,inline.} = - ## Subtracts `p2`from `p1` resulting in a difference vector. - result.x=p1.x-p2.x - result.y=p1.y-p2.y - -proc `-=`*(p:var Point2d,v:Vector2d) {.noInit,inline.} = - ## Subtracts a vector `v` from a point `p` in place. - p.x-=v.x - p.y-=v.y - -proc equals(p1,p2:Point2d,tol=1.0e-6):bool {.inline.}= - ## Checks if two points approximately equals with a tolerance. - return abs(p2.x-p1.x)<=tol and abs(p2.y-p1.y)<=tol - -proc `=~`*(p1,p2:Point2d):bool {.inline.}= - ## Checks if two vectors approximately equals with a - ## hardcoded tolerance 1e-6 - equals(p1,p2) - -proc polar*(p:Point2d,ang,dist:float):Point2d {.noInit.} = - ## Returns a point with a given angle and distance away from `p` - result.x=p.x+cos(ang)*dist - result.y=p.y+sin(ang)*dist - -proc rotate*(p:var Point2d,rad:float)= - ## Rotates a point in place `rad` radians around origo. - let - c=cos(rad) - s=sin(rad) - newx=p.x*c-p.y*s - p.y=p.y*c+p.x*s - p.x=newx - -proc rotate*(p:var Point2d,rad:float,org:Point2d)= - ## Rotates a point in place `rad` radians using `org` as - ## center of rotation. - let - c=cos(rad) - s=sin(rad) - newx=(p.x - org.x) * c - (p.y - org.y) * s + org.x - p.y=(p.y - org.y) * c + (p.x - org.x) * s + org.y - p.x=newx - -proc scale*(p:var Point2d,fac:float) {.inline.}= - ## Scales a point in place `fac` times with world origo as origin. - p.x*=fac - p.y*=fac - -proc scale*(p:var Point2d,fac:float,org:Point2d){.inline.}= - ## Scales the point in place `fac` times with `org` as origin. - p.x=(p.x - org.x) * fac + org.x - p.y=(p.y - org.y) * fac + org.y - -proc stretch*(p:var Point2d,facx,facy:float){.inline.}= - ## Scales a point in place non uniformly `facx` and `facy` times with - ## world origo as origin. - p.x*=facx - p.y*=facy - -proc stretch*(p:var Point2d,facx,facy:float,org:Point2d){.inline.}= - ## Scales the point in place non uniformly `facx` and `facy` times with - ## `org` as origin. - p.x=(p.x - org.x) * facx + org.x - p.y=(p.y - org.y) * facy + org.y - -proc move*(p:var Point2d,dx,dy:float){.inline.}= - ## Translates a point `dx`, `dy` in place. - p.x+=dx - p.y+=dy - -proc move*(p:var Point2d,v:Vector2d){.inline.}= - ## Translates a point with vector `v` in place. - p.x+=v.x - p.y+=v.y - -proc sgnArea*(a,b,c:Point2d):float= - ## Computes the signed area of the triangle thru points `a`,`b` and `c` - ## result>0.0 for counter clockwise triangle - ## result<0.0 for clockwise triangle - ## This is commonly used to determinate side of a point with respect to a line. - return ((b.x - c.x) * (b.y - a.y)-(b.y - c.y) * (b.x - a.x))*0.5 - -proc area*(a,b,c:Point2d):float= - ## Computes the area of the triangle thru points `a`,`b` and `c` - return abs(sgnArea(a,b,c)) - -proc closestPoint*(p:Point2d,pts:varargs[Point2d]):Point2d= - ## Returns a point selected from `pts`, that has the closest - ## euclidean distance to `p` - assert(pts.len>0) # must have at least one point - - var - bestidx=0 - bestdist=p.sqrDist(pts[0]) - curdist:float - - for idx in 1..high(pts): - curdist=p.sqrDist(pts[idx]) - if curdist<bestdist: - bestidx=idx - bestdist=curdist - - result=pts[bestidx] - - -# *************************************** -# Misc. math utilities that should -# probably be in another module. -# *************************************** -proc normAngle*(ang:float):float= - ## Returns an angle in radians, that is equal to `ang`, - ## but in the range 0 to <2*PI - if ang>=0.0 and ang<DEG360: - return ang - - return ang mod DEG360 - -proc degToRad*(deg:float):float {.inline.}= - ## converts `deg` degrees to radians - deg / RAD2DEGCONST - -proc radToDeg*(rad:float):float {.inline.}= - ## converts `rad` radians to degrees - rad * RAD2DEGCONST diff --git a/lib/pure/basic3d.nim b/lib/pure/basic3d.nim deleted file mode 100644 index e2d2464c0..000000000 --- a/lib/pure/basic3d.nim +++ /dev/null @@ -1,1040 +0,0 @@ -# -# -# Nim's Runtime Library -# (c) Copyright 2013 Robert Persson -# -# See the file "copying.txt", included in this -# distribution, for details about the copyright. -# - -import math -import strutils -import times - - -## Basic 3d support with vectors, points, matrices and some basic utilities. -## Vectors are implemented as direction vectors, ie. when transformed with a matrix -## the translation part of matrix is ignored. The coordinate system used is -## right handed, because its compatible with 2d coordinate system (rotation around -## zaxis equals 2d rotation). -## Operators `+` , `-` , `*` , `/` , `+=` , `-=` , `*=` and `/=` are implemented -## for vectors and scalars. -## -## -## Quick start example: -## -## .. code-block:: nim -## -## # Create a matrix which first rotates, then scales and at last translates -## -## var m:Matrix3d=rotate(PI,vector3d(1,1,2.5)) & scale(2.0) & move(100.0,200.0,300.0) -## -## # Create a 3d point at (100,150,200) and a vector (5,2,3) -## -## var pt:Point3d=point3d(100.0,150.0,200.0) -## -## var vec:Vector3d=vector3d(5.0,2.0,3.0) -## -## -## pt &= m # transforms pt in place -## -## var pt2:Point3d=pt & m #concatenates pt with m and returns a new point -## -## var vec2:Vector3d=vec & m #concatenates vec with m and returns a new vector - - - -type - Matrix3d* =object - ## Implements a row major 3d matrix, which means - ## transformations are applied the order they are concatenated. - ## This matrix is stored as an 4x4 matrix: - ## [ ax ay az aw ] - ## [ bx by bz bw ] - ## [ cx cy cz cw ] - ## [ tx ty tz tw ] - ax*,ay*,az*,aw*, bx*,by*,bz*,bw*, cx*,cy*,cz*,cw*, tx*,ty*,tz*,tw*:float - Point3d* = object - ## Implements a non-homogeneous 3d point stored as - ## an `x` , `y` and `z` coordinate. - x*,y*,z*:float - Vector3d* = object - ## Implements a 3d **direction vector** stored as - ## an `x` , `y` and `z` coordinate. Direction vector means, - ## that when transforming a vector with a matrix, the translational - ## part of the matrix is ignored. - x*,y*,z*:float -{.deprecated: [TMatrix3d: Matrix3d, TPoint3d: Point3d, TVector3d: Vector3d].} - - -# Some forward declarations -proc matrix3d*(ax,ay,az,aw,bx,by,bz,bw,cx,cy,cz,cw,tx,ty,tz,tw:float):Matrix3d {.noInit.} - ## Creates a new 4x4 3d transformation matrix. - ## `ax` , `ay` , `az` is the local x axis. - ## `bx` , `by` , `bz` is the local y axis. - ## `cx` , `cy` , `cz` is the local z axis. - ## `tx` , `ty` , `tz` is the translation. -proc vector3d*(x,y,z:float):Vector3d {.noInit,inline.} - ## Returns a new 3d vector (`x`,`y`,`z`) -proc point3d*(x,y,z:float):Point3d {.noInit,inline.} - ## Returns a new 4d point (`x`,`y`,`z`) -proc tryNormalize*(v:var Vector3d):bool - ## Modifies `v` to have a length of 1.0, keeping its angle. - ## If `v` has zero length (and thus no angle), it is left unmodified and false is - ## returned, otherwise true is returned. - - - -let - IDMATRIX*:Matrix3d=matrix3d( - 1.0,0.0,0.0,0.0, - 0.0,1.0,0.0,0.0, - 0.0,0.0,1.0,0.0, - 0.0,0.0,0.0,1.0) - ## Quick access to a 3d identity matrix - ORIGO*:Point3d=point3d(0.0,0.0,0.0) - ## Quick access to point (0,0) - XAXIS*:Vector3d=vector3d(1.0,0.0,0.0) - ## Quick access to an 3d x-axis unit vector - YAXIS*:Vector3d=vector3d(0.0,1.0,0.0) - ## Quick access to an 3d y-axis unit vector - ZAXIS*:Vector3d=vector3d(0.0,0.0,1.0) - ## Quick access to an 3d z-axis unit vector - - - -# *************************************** -# Private utils -# *************************************** - -proc rtos(val:float):string= - return formatFloat(val,ffDefault,0) - -proc safeArccos(v:float):float= - ## assumes v is in range 0.0-1.0, but clamps - ## the value to avoid out of domain errors - ## due to rounding issues - return arccos(clamp(v,-1.0,1.0)) - -template makeBinOpVector(s) = - proc s*(a,b:Vector3d):Vector3d {.inline,noInit.} = - vector3d(s(a.x,b.x),s(a.y,b.y),s(a.z,b.z)) - proc s*(a:Vector3d,b:float):Vector3d {.inline,noInit.} = - vector3d(s(a.x,b),s(a.y,b),s(a.z,b)) - proc s*(a:float,b:Vector3d):Vector3d {.inline,noInit.} = - vector3d(s(a,b.x),s(a,b.y),s(a,b.z)) - -template makeBinOpAssignVector(s) = - proc s*(a:var Vector3d,b:Vector3d) {.inline.} = - s(a.x,b.x); s(a.y,b.y); s(a.z,b.z) - proc s*(a:var Vector3d,b:float) {.inline.} = - s(a.x,b); s(a.y,b); s(a.z,b) - - - -# *************************************** -# Matrix3d implementation -# *************************************** - -proc setElements*(t:var Matrix3d,ax,ay,az,aw,bx,by,bz,bw,cx,cy,cz,cw,tx,ty,tz,tw:float) {.inline.}= - ## Sets arbitrary elements in an exisitng matrix. - t.ax=ax - t.ay=ay - t.az=az - t.aw=aw - t.bx=bx - t.by=by - t.bz=bz - t.bw=bw - t.cx=cx - t.cy=cy - t.cz=cz - t.cw=cw - t.tx=tx - t.ty=ty - t.tz=tz - t.tw=tw - -proc matrix3d*(ax,ay,az,aw,bx,by,bz,bw,cx,cy,cz,cw,tx,ty,tz,tw:float):Matrix3d = - result.setElements(ax,ay,az,aw,bx,by,bz,bw,cx,cy,cz,cw,tx,ty,tz,tw) - -proc `&`*(a,b:Matrix3d):Matrix3d {.noinit.} = - ## Concatenates matrices returning a new matrix. - result.setElements( - a.aw*b.tx+a.az*b.cx+a.ay*b.bx+a.ax*b.ax, - a.aw*b.ty+a.az*b.cy+a.ay*b.by+a.ax*b.ay, - a.aw*b.tz+a.az*b.cz+a.ay*b.bz+a.ax*b.az, - a.aw*b.tw+a.az*b.cw+a.ay*b.bw+a.ax*b.aw, - - a.bw*b.tx+a.bz*b.cx+a.by*b.bx+a.bx*b.ax, - a.bw*b.ty+a.bz*b.cy+a.by*b.by+a.bx*b.ay, - a.bw*b.tz+a.bz*b.cz+a.by*b.bz+a.bx*b.az, - a.bw*b.tw+a.bz*b.cw+a.by*b.bw+a.bx*b.aw, - - a.cw*b.tx+a.cz*b.cx+a.cy*b.bx+a.cx*b.ax, - a.cw*b.ty+a.cz*b.cy+a.cy*b.by+a.cx*b.ay, - a.cw*b.tz+a.cz*b.cz+a.cy*b.bz+a.cx*b.az, - a.cw*b.tw+a.cz*b.cw+a.cy*b.bw+a.cx*b.aw, - - a.tw*b.tx+a.tz*b.cx+a.ty*b.bx+a.tx*b.ax, - a.tw*b.ty+a.tz*b.cy+a.ty*b.by+a.tx*b.ay, - a.tw*b.tz+a.tz*b.cz+a.ty*b.bz+a.tx*b.az, - a.tw*b.tw+a.tz*b.cw+a.ty*b.bw+a.tx*b.aw) - - -proc scale*(s:float):Matrix3d {.noInit.} = - ## Returns a new scaling matrix. - result.setElements(s,0,0,0, 0,s,0,0, 0,0,s,0, 0,0,0,1) - -proc scale*(s:float,org:Point3d):Matrix3d {.noInit.} = - ## Returns a new scaling matrix using, `org` as scale origin. - result.setElements(s,0,0,0, 0,s,0,0, 0,0,s,0, - org.x-s*org.x,org.y-s*org.y,org.z-s*org.z,1.0) - -proc stretch*(sx,sy,sz:float):Matrix3d {.noInit.} = - ## Returns new a stretch matrix, which is a - ## scale matrix with non uniform scale in x,y and z. - result.setElements(sx,0,0,0, 0,sy,0,0, 0,0,sz,0, 0,0,0,1) - -proc stretch*(sx,sy,sz:float,org:Point3d):Matrix3d {.noInit.} = - ## Returns a new stretch matrix, which is a - ## scale matrix with non uniform scale in x,y and z. - ## `org` is used as stretch origin. - result.setElements(sx,0,0,0, 0,sy,0,0, 0,0,sz,0, org.x-sx*org.x,org.y-sy*org.y,org.z-sz*org.z,1) - -proc move*(dx,dy,dz:float):Matrix3d {.noInit.} = - ## Returns a new translation matrix. - result.setElements(1,0,0,0, 0,1,0,0, 0,0,1,0, dx,dy,dz,1) - -proc move*(v:Vector3d):Matrix3d {.noInit.} = - ## Returns a new translation matrix from a vector. - result.setElements(1,0,0,0, 0,1,0,0, 0,0,1,0, v.x,v.y,v.z,1) - - -proc rotate*(angle:float,axis:Vector3d):Matrix3d {.noInit.}= - ## Creates a rotation matrix that rotates `angle` radians over - ## `axis`, which passes through origo. - - # see PDF document http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.pdf - # for how this is computed - - var normax=axis - if not normax.tryNormalize: #simplifies matrix computation below a lot - raise newException(DivByZeroError,"Cannot rotate around zero length axis") - - let - cs=cos(angle) - si=sin(angle) - omc=1.0-cs - usi=normax.x*si - vsi=normax.y*si - wsi=normax.z*si - u2=normax.x*normax.x - v2=normax.y*normax.y - w2=normax.z*normax.z - uvomc=normax.x*normax.y*omc - uwomc=normax.x*normax.z*omc - vwomc=normax.y*normax.z*omc - - result.setElements( - u2+(1.0-u2)*cs, uvomc+wsi, uwomc-vsi, 0.0, - uvomc-wsi, v2+(1.0-v2)*cs, vwomc+usi, 0.0, - uwomc+vsi, vwomc-usi, w2+(1.0-w2)*cs, 0.0, - 0.0,0.0,0.0,1.0) - -proc rotate*(angle:float,org:Point3d,axis:Vector3d):Matrix3d {.noInit.}= - ## Creates a rotation matrix that rotates `angle` radians over - ## `axis`, which passes through `org`. - - # see PDF document http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.pdf - # for how this is computed - - var normax=axis - if not normax.tryNormalize: #simplifies matrix computation below a lot - raise newException(DivByZeroError,"Cannot rotate around zero length axis") - - let - u=normax.x - v=normax.y - w=normax.z - u2=u*u - v2=v*v - w2=w*w - cs=cos(angle) - omc=1.0-cs - si=sin(angle) - a=org.x - b=org.y - c=org.z - usi=u*si - vsi=v*si - wsi=w*si - uvomc=normax.x*normax.y*omc - uwomc=normax.x*normax.z*omc - vwomc=normax.y*normax.z*omc - - result.setElements( - u2+(v2+w2)*cs, uvomc+wsi, uwomc-vsi, 0.0, - uvomc-wsi, v2+(u2+w2)*cs, vwomc+usi, 0.0, - uwomc+vsi, vwomc-usi, w2+(u2+v2)*cs, 0.0, - (a*(v2+w2)-u*(b*v+c*w))*omc+(b*w-c*v)*si, - (b*(u2+w2)-v*(a*u+c*w))*omc+(c*u-a*w)*si, - (c*(u2+v2)-w*(a*u+b*v))*omc+(a*v-b*u)*si,1.0) - - -proc rotateX*(angle:float):Matrix3d {.noInit.}= - ## Creates a matrix that rotates around the x-axis with `angle` radians, - ## which is also called a 'roll' matrix. - let - c=cos(angle) - s=sin(angle) - result.setElements( - 1,0,0,0, - 0,c,s,0, - 0,-s,c,0, - 0,0,0,1) - -proc rotateY*(angle:float):Matrix3d {.noInit.}= - ## Creates a matrix that rotates around the y-axis with `angle` radians, - ## which is also called a 'pitch' matrix. - let - c=cos(angle) - s=sin(angle) - result.setElements( - c,0,-s,0, - 0,1,0,0, - s,0,c,0, - 0,0,0,1) - -proc rotateZ*(angle:float):Matrix3d {.noInit.}= - ## Creates a matrix that rotates around the z-axis with `angle` radians, - ## which is also called a 'yaw' matrix. - let - c=cos(angle) - s=sin(angle) - result.setElements( - c,s,0,0, - -s,c,0,0, - 0,0,1,0, - 0,0,0,1) - -proc isUniform*(m:Matrix3d,tol=1.0e-6):bool= - ## Checks if the transform is uniform, that is - ## perpendicular axes of equal length, which means (for example) - ## it cannot transform a sphere into an ellipsoid. - ## `tol` is used as tolerance for both equal length comparison - ## and perpendicular comparison. - - #dot product=0 means perpendicular coord. system, check xaxis vs yaxis and xaxis vs zaxis - if abs(m.ax*m.bx+m.ay*m.by+m.az*m.bz)<=tol and # x vs y - abs(m.ax*m.cx+m.ay*m.cy+m.az*m.cz)<=tol and #x vs z - abs(m.bx*m.cx+m.by*m.cy+m.bz*m.cz)<=tol: #y vs z - - #subtract squared lengths of axes to check if uniform scaling: - let - sqxlen=(m.ax*m.ax+m.ay*m.ay+m.az*m.az) - sqylen=(m.bx*m.bx+m.by*m.by+m.bz*m.bz) - sqzlen=(m.cx*m.cx+m.cy*m.cy+m.cz*m.cz) - if abs(sqxlen-sqylen)<=tol and abs(sqxlen-sqzlen)<=tol: - return true - return false - - - -proc mirror*(planeperp:Vector3d):Matrix3d {.noInit.}= - ## Creates a matrix that mirrors over the plane that has `planeperp` as normal, - ## and passes through origo. `planeperp` does not need to be normalized. - - # https://en.wikipedia.org/wiki/Transformation_matrix - var n=planeperp - if not n.tryNormalize: - raise newException(DivByZeroError,"Cannot mirror over a plane with a zero length normal") - - let - a=n.x - b=n.y - c=n.z - ab=a*b - ac=a*c - bc=b*c - - result.setElements( - 1-2*a*a , -2*ab,-2*ac,0, - -2*ab , 1-2*b*b, -2*bc, 0, - -2*ac, -2*bc, 1-2*c*c,0, - 0,0,0,1) - - -proc mirror*(org:Point3d,planeperp:Vector3d):Matrix3d {.noInit.}= - ## Creates a matrix that mirrors over the plane that has `planeperp` as normal, - ## and passes through `org`. `planeperp` does not need to be normalized. - - # constructs a mirror M like the simpler mirror matrix constructor - # above but premultiplies with the inverse traslation of org - # and postmultiplies with the translation of org. - # With some fiddling this becomes reasonably simple: - var n=planeperp - if not n.tryNormalize: - raise newException(DivByZeroError,"Cannot mirror over a plane with a zero length normal") - - let - a=n.x - b=n.y - c=n.z - ab=a*b - ac=a*c - bc=b*c - aa=a*a - bb=b*b - cc=c*c - tx=org.x - ty=org.y - tz=org.z - - result.setElements( - 1-2*aa , -2*ab,-2*ac,0, - -2*ab , 1-2*bb, -2*bc, 0, - -2*ac, -2*bc, 1-2*cc,0, - 2*(ac*tz+ab*ty+aa*tx), - 2*(bc*tz+bb*ty+ab*tx), - 2*(cc*tz+bc*ty+ac*tx) ,1) - - -proc determinant*(m:Matrix3d):float= - ## Computes the determinant of matrix `m`. - - # This computation is gotten from ratsimp(optimize(determinant(m))) - # in maxima CAS - let - O1=m.cx*m.tw-m.cw*m.tx - O2=m.cy*m.tw-m.cw*m.ty - O3=m.cx*m.ty-m.cy*m.tx - O4=m.cz*m.tw-m.cw*m.tz - O5=m.cx*m.tz-m.cz*m.tx - O6=m.cy*m.tz-m.cz*m.ty - - return (O1*m.ay-O2*m.ax-O3*m.aw)*m.bz+ - (-O1*m.az+O4*m.ax+O5*m.aw)*m.by+ - (O2*m.az-O4*m.ay-O6*m.aw)*m.bx+ - (O3*m.az-O5*m.ay+O6*m.ax)*m.bw - - -proc inverse*(m:Matrix3d):Matrix3d {.noInit.}= - ## Computes the inverse of matrix `m`. If the matrix - ## determinant is zero, thus not invertible, a EDivByZero - ## will be raised. - - # this computation comes from optimize(invert(m)) in maxima CAS - - let - det=m.determinant - O2=m.cy*m.tw-m.cw*m.ty - O3=m.cz*m.tw-m.cw*m.tz - O4=m.cy*m.tz-m.cz*m.ty - O5=m.by*m.tw-m.bw*m.ty - O6=m.bz*m.tw-m.bw*m.tz - O7=m.by*m.tz-m.bz*m.ty - O8=m.by*m.cw-m.bw*m.cy - O9=m.bz*m.cw-m.bw*m.cz - O10=m.by*m.cz-m.bz*m.cy - O11=m.cx*m.tw-m.cw*m.tx - O12=m.cx*m.tz-m.cz*m.tx - O13=m.bx*m.tw-m.bw*m.tx - O14=m.bx*m.tz-m.bz*m.tx - O15=m.bx*m.cw-m.bw*m.cx - O16=m.bx*m.cz-m.bz*m.cx - O17=m.cx*m.ty-m.cy*m.tx - O18=m.bx*m.ty-m.by*m.tx - O19=m.bx*m.cy-m.by*m.cx - - if det==0.0: - raise newException(DivByZeroError,"Cannot normalize zero length vector") - - result.setElements( - (m.bw*O4+m.by*O3-m.bz*O2)/det , (-m.aw*O4-m.ay*O3+m.az*O2)/det, - (m.aw*O7+m.ay*O6-m.az*O5)/det , (-m.aw*O10-m.ay*O9+m.az*O8)/det, - (-m.bw*O12-m.bx*O3+m.bz*O11)/det , (m.aw*O12+m.ax*O3-m.az*O11)/det, - (-m.aw*O14-m.ax*O6+m.az*O13)/det , (m.aw*O16+m.ax*O9-m.az*O15)/det, - (m.bw*O17+m.bx*O2-m.by*O11)/det , (-m.aw*O17-m.ax*O2+m.ay*O11)/det, - (m.aw*O18+m.ax*O5-m.ay*O13)/det , (-m.aw*O19-m.ax*O8+m.ay*O15)/det, - (-m.bx*O4+m.by*O12-m.bz*O17)/det , (m.ax*O4-m.ay*O12+m.az*O17)/det, - (-m.ax*O7+m.ay*O14-m.az*O18)/det , (m.ax*O10-m.ay*O16+m.az*O19)/det) - - -proc equals*(m1:Matrix3d,m2:Matrix3d,tol=1.0e-6):bool= - ## Checks if all elements of `m1`and `m2` is equal within - ## a given tolerance `tol`. - return - abs(m1.ax-m2.ax)<=tol and - abs(m1.ay-m2.ay)<=tol and - abs(m1.az-m2.az)<=tol and - abs(m1.aw-m2.aw)<=tol and - abs(m1.bx-m2.bx)<=tol and - abs(m1.by-m2.by)<=tol and - abs(m1.bz-m2.bz)<=tol and - abs(m1.bw-m2.bw)<=tol and - abs(m1.cx-m2.cx)<=tol and - abs(m1.cy-m2.cy)<=tol and - abs(m1.cz-m2.cz)<=tol and - abs(m1.cw-m2.cw)<=tol and - abs(m1.tx-m2.tx)<=tol and - abs(m1.ty-m2.ty)<=tol and - abs(m1.tz-m2.tz)<=tol and - abs(m1.tw-m2.tw)<=tol - -proc `=~`*(m1,m2:Matrix3d):bool= - ## Checks if `m1` and `m2` is approximately equal, using a - ## tolerance of 1e-6. - equals(m1,m2) - -proc transpose*(m:Matrix3d):Matrix3d {.noInit.}= - ## Returns the transpose of `m` - result.setElements(m.ax,m.bx,m.cx,m.tx,m.ay,m.by,m.cy,m.ty,m.az,m.bz,m.cz,m.tz,m.aw,m.bw,m.cw,m.tw) - -proc getXAxis*(m:Matrix3d):Vector3d {.noInit.}= - ## Gets the local x axis of `m` - result.x=m.ax - result.y=m.ay - result.z=m.az - -proc getYAxis*(m:Matrix3d):Vector3d {.noInit.}= - ## Gets the local y axis of `m` - result.x=m.bx - result.y=m.by - result.z=m.bz - -proc getZAxis*(m:Matrix3d):Vector3d {.noInit.}= - ## Gets the local y axis of `m` - result.x=m.cx - result.y=m.cy - result.z=m.cz - - -proc `$`*(m:Matrix3d):string= - ## String representation of `m` - return rtos(m.ax) & "," & rtos(m.ay) & "," & rtos(m.az) & "," & rtos(m.aw) & - "\n" & rtos(m.bx) & "," & rtos(m.by) & "," & rtos(m.bz) & "," & rtos(m.bw) & - "\n" & rtos(m.cx) & "," & rtos(m.cy) & "," & rtos(m.cz) & "," & rtos(m.cw) & - "\n" & rtos(m.tx) & "," & rtos(m.ty) & "," & rtos(m.tz) & "," & rtos(m.tw) - -proc apply*(m:Matrix3d, x,y,z:var float, translate=false)= - ## Applies transformation `m` onto `x` , `y` , `z` , optionally - ## using the translation part of the matrix. - let - oldx=x - oldy=y - oldz=z - - x=m.cx*oldz+m.bx*oldy+m.ax*oldx - y=m.cy*oldz+m.by*oldy+m.ay*oldx - z=m.cz*oldz+m.bz*oldy+m.az*oldx - - if translate: - x+=m.tx - y+=m.ty - z+=m.tz - -# *************************************** -# Vector3d implementation -# *************************************** -proc vector3d*(x,y,z:float):Vector3d= - result.x=x - result.y=y - result.z=z - -proc len*(v:Vector3d):float= - ## Returns the length of the vector `v`. - sqrt(v.x*v.x+v.y*v.y+v.z*v.z) - -proc `len=`*(v:var Vector3d,newlen:float) {.noInit.} = - ## Sets the length of the vector, keeping its direction. - ## If the vector has zero length before changing it's length, - ## an arbitrary vector of the requested length is returned. - - let fac=newlen/v.len - - if newlen==0.0: - v.x=0.0 - v.y=0.0 - v.z=0.0 - return - - if fac==Inf or fac==NegInf: - #to short for float accuracy - #do as good as possible: - v.x=newlen - v.y=0.0 - v.z=0.0 - else: - v.x*=fac - v.y*=fac - v.z*=fac - - -proc sqrLen*(v:Vector3d):float {.inline.}= - ## Computes the squared length of the vector, which is - ## faster than computing the absolute length. - return v.x*v.x+v.y*v.y+v.z*v.z - -proc `$` *(v:Vector3d):string= - ## String representation of `v` - result=rtos(v.x) - result.add(",") - result.add(rtos(v.y)) - result.add(",") - result.add(rtos(v.z)) - -proc `&` *(v:Vector3d,m:Matrix3d):Vector3d {.noInit.} = - ## Concatenate vector `v` with a transformation matrix. - ## Transforming a vector ignores the translational part - ## of the matrix. - - # | AX AY AZ AW | - # | X Y Z 1 | * | BX BY BZ BW | - # | CX CY CZ CW | - # | 0 0 0 1 | - let - newx=m.cx*v.z+m.bx*v.y+m.ax*v.x - newy=m.cy*v.z+m.by*v.y+m.ay*v.x - result.z=m.cz*v.z+m.bz*v.y+m.az*v.x - result.y=newy - result.x=newx - - -proc `&=` *(v:var Vector3d,m:Matrix3d) {.noInit.} = - ## Applies transformation `m` onto `v` in place. - ## Transforming a vector ignores the translational part - ## of the matrix. - - # | AX AY AZ AW | - # | X Y Z 1 | * | BX BY BZ BW | - # | CX CY CZ CW | - # | 0 0 0 1 | - - let - newx=m.cx*v.z+m.bx*v.y+m.ax*v.x - newy=m.cy*v.z+m.by*v.y+m.ay*v.x - v.z=m.cz*v.z+m.bz*v.y+m.az*v.x - v.y=newy - v.x=newx - -proc transformNorm*(v:var Vector3d,m:Matrix3d)= - ## Applies a normal direction transformation `m` onto `v` in place. - ## The resulting vector is *not* normalized. Transforming a vector ignores the - ## translational part of the matrix. If the matrix is not invertible - ## (determinant=0), an EDivByZero will be raised. - - # transforming a normal is done by transforming - # by the transpose of the inverse of the original matrix - - # Major reason this simple function is here is that this function can be optimized in the future, - # (possibly by hardware) as well as having a consistent API with the 2d version. - v&=transpose(inverse(m)) - -proc transformInv*(v:var Vector3d,m:Matrix3d)= - ## Applies the inverse of `m` on vector `v`. Transforming a vector ignores - ## the translational part of the matrix. Transforming a vector ignores the - ## translational part of the matrix. - ## If the matrix is not invertible (determinant=0), an EDivByZero - ## will be raised. - - # Major reason this simple function is here is that this function can be optimized in the future, - # (possibly by hardware) as well as having a consistent API with the 2d version. - v&=m.inverse - -proc transformNormInv*(vec:var Vector3d,m:Matrix3d)= - ## Applies an inverse normal direction transformation `m` onto `v` in place. - ## This is faster than creating an inverse - ## matrix and transformNorm(...) it. Transforming a vector ignores the - ## translational part of the matrix. - - # see vector2d:s equivalent for a deeper look how/why this works - vec&=m.transpose - -proc tryNormalize*(v:var Vector3d):bool= - ## Modifies `v` to have a length of 1.0, keeping its angle. - ## If `v` has zero length (and thus no angle), it is left unmodified and false is - ## returned, otherwise true is returned. - let mag=v.len - - if mag==0.0: - return false - - v.x/=mag - v.y/=mag - v.z/=mag - - return true - -proc normalize*(v:var Vector3d) {.inline.}= - ## Modifies `v` to have a length of 1.0, keeping its angle. - ## If `v` has zero length, an EDivByZero will be raised. - if not tryNormalize(v): - raise newException(DivByZeroError,"Cannot normalize zero length vector") - -proc rotate*(vec:var Vector3d,angle:float,axis:Vector3d)= - ## Rotates `vec` in place, with `angle` radians over `axis`, which passes - ## through origo. - - # see PDF document http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.pdf - # for how this is computed - - var normax=axis - if not normax.tryNormalize: - raise newException(DivByZeroError,"Cannot rotate around zero length axis") - - let - cs=cos(angle) - si=sin(angle) - omc=1.0-cs - u=normax.x - v=normax.y - w=normax.z - x=vec.x - y=vec.y - z=vec.z - uxyzomc=(u*x+v*y+w*z)*omc - - vec.x=u*uxyzomc+x*cs+(v*z-w*y)*si - vec.y=v*uxyzomc+y*cs+(w*x-u*z)*si - vec.z=w*uxyzomc+z*cs+(u*y-v*x)*si - -proc scale*(v:var Vector3d,s:float)= - ## Scales the vector in place with factor `s` - v.x*=s - v.y*=s - v.z*=s - -proc stretch*(v:var Vector3d,sx,sy,sz:float)= - ## Scales the vector non uniformly with factors `sx` , `sy` , `sz` - v.x*=sx - v.y*=sy - v.z*=sz - -proc mirror*(v:var Vector3d,planeperp:Vector3d)= - ## Computes the mirrored vector of `v` over the plane - ## that has `planeperp` as normal direction. - ## `planeperp` does not need to be normalized. - - var n=planeperp - n.normalize - - let - x=v.x - y=v.y - z=v.z - a=n.x - b=n.y - c=n.z - ac=a*c - ab=a*b - bc=b*c - - v.x= -2*(ac*z+ab*y+a*a*x)+x - v.y= -2*(bc*z+b*b*y+ab*x)+y - v.z= -2*(c*c*z+bc*y+ac*x)+z - - -proc `-` *(v:Vector3d):Vector3d= - ## Negates a vector - result.x= -v.x - result.y= -v.y - result.z= -v.z - -# declare templated binary operators -makeBinOpVector(`+`) -makeBinOpVector(`-`) -makeBinOpVector(`*`) -makeBinOpVector(`/`) -makeBinOpAssignVector(`+=`) -makeBinOpAssignVector(`-=`) -makeBinOpAssignVector(`*=`) -makeBinOpAssignVector(`/=`) - -proc dot*(v1,v2:Vector3d):float {.inline.}= - ## Computes the dot product of two vectors. - ## Returns 0.0 if the vectors are perpendicular. - return v1.x*v2.x+v1.y*v2.y+v1.z*v2.z - -proc cross*(v1,v2:Vector3d):Vector3d {.inline.}= - ## Computes the cross product of two vectors. - ## The result is a vector which is perpendicular - ## to the plane of `v1` and `v2`, which means - ## cross(xaxis,yaxis)=zaxis. The magnitude of the result is - ## zero if the vectors are colinear. - result.x = (v1.y * v2.z) - (v2.y * v1.z) - result.y = (v1.z * v2.x) - (v2.z * v1.x) - result.z = (v1.x * v2.y) - (v2.x * v1.y) - -proc equals*(v1,v2:Vector3d,tol=1.0e-6):bool= - ## Checks if two vectors approximately equals with a tolerance. - return abs(v2.x-v1.x)<=tol and abs(v2.y-v1.y)<=tol and abs(v2.z-v1.z)<=tol - -proc `=~` *(v1,v2:Vector3d):bool= - ## Checks if two vectors approximately equals with a - ## hardcoded tolerance 1e-6 - equals(v1,v2) - -proc angleTo*(v1,v2:Vector3d):float= - ## Returns the smallest angle between v1 and v2, - ## which is in range 0-PI - var - nv1=v1 - nv2=v2 - if not nv1.tryNormalize or not nv2.tryNormalize: - return 0.0 # zero length vector has zero angle to any other vector - return safeArccos(dot(nv1,nv2)) - -proc arbitraryAxis*(norm:Vector3d):Matrix3d {.noInit.}= - ## Computes the rotation matrix that would transform - ## world z vector into `norm`. The inverse of this matrix - ## is useful to transform a planar 3d object to 2d space. - ## This is the same algorithm used to interpret DXF and DWG files. - const lim=1.0/64.0 - var ax,ay,az:Vector3d - if abs(norm.x)<lim and abs(norm.y)<lim: - ax=cross(YAXIS,norm) - else: - ax=cross(ZAXIS,norm) - - ax.normalize() - ay=cross(norm,ax) - ay.normalize() - az=cross(ax,ay) - - result.setElements( - ax.x,ax.y,ax.z,0.0, - ay.x,ay.y,ay.z,0.0, - az.x,az.y,az.z,0.0, - 0.0,0.0,0.0,1.0) - -proc bisect*(v1,v2:Vector3d):Vector3d {.noInit.}= - ## Computes the bisector between v1 and v2 as a normalized vector. - ## If one of the input vectors has zero length, a normalized version - ## of the other is returned. If both input vectors has zero length, - ## an arbitrary normalized vector `v1` is returned. - var - vmag1=v1.len - vmag2=v2.len - - # zero length vector equals arbitrary vector, just change - # magnitude to one to avoid zero division - if vmag1==0.0: - if vmag2==0: #both are zero length return any normalized vector - return XAXIS - vmag1=1.0 - if vmag2==0.0: vmag2=1.0 - - let - x1=v1.x/vmag1 - y1=v1.y/vmag1 - z1=v1.z/vmag1 - x2=v2.x/vmag2 - y2=v2.y/vmag2 - z2=v2.z/vmag2 - - result.x=(x1 + x2) * 0.5 - result.y=(y1 + y2) * 0.5 - result.z=(z1 + z2) * 0.5 - - if not result.tryNormalize(): - # This can happen if vectors are colinear. In this special case - # there are actually inifinitely many bisectors, we select just - # one of them. - result=v1.cross(XAXIS) - if result.sqrLen<1.0e-9: - result=v1.cross(YAXIS) - if result.sqrLen<1.0e-9: - result=v1.cross(ZAXIS) # now we should be guaranteed to have succeeded - result.normalize - - - -# *************************************** -# Point3d implementation -# *************************************** -proc point3d*(x,y,z:float):Point3d= - result.x=x - result.y=y - result.z=z - -proc sqrDist*(a,b:Point3d):float= - ## Computes the squared distance between `a`and `b` - let dx=b.x-a.x - let dy=b.y-a.y - let dz=b.z-a.z - result=dx*dx+dy*dy+dz*dz - -proc dist*(a,b:Point3d):float {.inline.}= - ## Computes the absolute distance between `a`and `b` - result=sqrt(sqrDist(a,b)) - -proc `$` *(p:Point3d):string= - ## String representation of `p` - result=rtos(p.x) - result.add(",") - result.add(rtos(p.y)) - result.add(",") - result.add(rtos(p.z)) - -proc `&`*(p:Point3d,m:Matrix3d):Point3d= - ## Concatenates a point `p` with a transform `m`, - ## resulting in a new, transformed point. - result.z=m.cz*p.z+m.bz*p.y+m.az*p.x+m.tz - result.y=m.cy*p.z+m.by*p.y+m.ay*p.x+m.ty - result.x=m.cx*p.z+m.bx*p.y+m.ax*p.x+m.tx - -proc `&=` *(p:var Point3d,m:Matrix3d)= - ## Applies transformation `m` onto `p` in place. - let - x=p.x - y=p.y - z=p.z - p.x=m.cx*z+m.bx*y+m.ax*x+m.tx - p.y=m.cy*z+m.by*y+m.ay*x+m.ty - p.z=m.cz*z+m.bz*y+m.az*x+m.tz - -proc transformInv*(p:var Point3d,m:Matrix3d)= - ## Applies the inverse of transformation `m` onto `p` in place. - ## If the matrix is not invertable (determinant=0) , EDivByZero will - ## be raised. - - # can possibly be more optimized in the future so use this function when possible - p&=inverse(m) - - -proc `+`*(p:Point3d,v:Vector3d):Point3d {.noInit,inline.} = - ## Adds a vector `v` to a point `p`, resulting - ## in a new point. - result.x=p.x+v.x - result.y=p.y+v.y - result.z=p.z+v.z - -proc `+=`*(p:var Point3d,v:Vector3d) {.noInit,inline.} = - ## Adds a vector `v` to a point `p` in place. - p.x+=v.x - p.y+=v.y - p.z+=v.z - -proc `-`*(p:Point3d,v:Vector3d):Point3d {.noInit,inline.} = - ## Subtracts a vector `v` from a point `p`, resulting - ## in a new point. - result.x=p.x-v.x - result.y=p.y-v.y - result.z=p.z-v.z - -proc `-`*(p1,p2:Point3d):Vector3d {.noInit,inline.} = - ## Subtracts `p2`from `p1` resulting in a difference vector. - result.x=p1.x-p2.x - result.y=p1.y-p2.y - result.z=p1.z-p2.z - -proc `-=`*(p:var Point3d,v:Vector3d) {.noInit,inline.} = - ## Subtracts a vector `v` from a point `p` in place. - p.x-=v.x - p.y-=v.y - p.z-=v.z - -proc equals(p1,p2:Point3d,tol=1.0e-6):bool {.inline.}= - ## Checks if two points approximately equals with a tolerance. - return abs(p2.x-p1.x)<=tol and abs(p2.y-p1.y)<=tol and abs(p2.z-p1.z)<=tol - -proc `=~`*(p1,p2:Point3d):bool {.inline.}= - ## Checks if two vectors approximately equals with a - ## hardcoded tolerance 1e-6 - equals(p1,p2) - -proc rotate*(p:var Point3d,rad:float,axis:Vector3d)= - ## Rotates point `p` in place `rad` radians about an axis - ## passing through origo. - - var v=vector3d(p.x,p.y,p.z) - v.rotate(rad,axis) # reuse this code here since doing the same thing and quite complicated - p.x=v.x - p.y=v.y - p.z=v.z - -proc rotate*(p:var Point3d,angle:float,org:Point3d,axis:Vector3d)= - ## Rotates point `p` in place `rad` radians about an axis - ## passing through `org` - - # see PDF document http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.pdf - # for how this is computed - - var normax=axis - normax.normalize - - let - cs=cos(angle) - omc=1.0-cs - si=sin(angle) - u=normax.x - v=normax.y - w=normax.z - a=org.x - b=org.y - c=org.z - x=p.x - y=p.y - z=p.z - uu=u*u - vv=v*v - ww=w*w - ux=u*p.x - vy=v*p.y - wz=w*p.z - au=a*u - bv=b*v - cw=c*w - uxmvymwz=ux-vy-wz - - p.x=(a*(vv+ww)-u*(bv+cw-uxmvymwz))*omc + x*cs + (b*w+v*z-c*v-w*y)*si - p.y=(b*(uu+ww)-v*(au+cw-uxmvymwz))*omc + y*cs + (c*u-a*w+w*x-u*z)*si - p.z=(c*(uu+vv)-w*(au+bv-uxmvymwz))*omc + z*cs + (a*v+u*y-b*u-v*x)*si - -proc scale*(p:var Point3d,fac:float) {.inline.}= - ## Scales a point in place `fac` times with world origo as origin. - p.x*=fac - p.y*=fac - p.z*=fac - -proc scale*(p:var Point3d,fac:float,org:Point3d){.inline.}= - ## Scales the point in place `fac` times with `org` as origin. - p.x=(p.x - org.x) * fac + org.x - p.y=(p.y - org.y) * fac + org.y - p.z=(p.z - org.z) * fac + org.z - -proc stretch*(p:var Point3d,facx,facy,facz:float){.inline.}= - ## Scales a point in place non uniformly `facx` , `facy` , `facz` times - ## with world origo as origin. - p.x*=facx - p.y*=facy - p.z*=facz - -proc stretch*(p:var Point3d,facx,facy,facz:float,org:Point3d){.inline.}= - ## Scales the point in place non uniformly `facx` , `facy` , `facz` times - ## with `org` as origin. - p.x=(p.x - org.x) * facx + org.x - p.y=(p.y - org.y) * facy + org.y - p.z=(p.z - org.z) * facz + org.z - - -proc move*(p:var Point3d,dx,dy,dz:float){.inline.}= - ## Translates a point `dx` , `dy` , `dz` in place. - p.x+=dx - p.y+=dy - p.z+=dz - -proc move*(p:var Point3d,v:Vector3d){.inline.}= - ## Translates a point with vector `v` in place. - p.x+=v.x - p.y+=v.y - p.z+=v.z - -proc area*(a,b,c:Point3d):float {.inline.}= - ## Computes the area of the triangle thru points `a` , `b` and `c` - - # The area of a planar 3d quadliteral is the magnitude of the cross - # product of two edge vectors. Taking this time 0.5 gives the triangle area. - return cross(b-a,c-a).len*0.5 - |