Maths3D code for games. Originally based on code posted by Mark Sibly:
Strict Import BRL.Math Import BRL.Blitz Function Vec2:TVec2(x#=0.0,y#=0.0/0.0) Local t:TVec2=New TVec2 If IsNan(y) y=x t.x=x t.y=y Return t EndFunction Function Vec3:TVec3( x#=0.0,y#=0.0/0.0,z#=0.0/0.0 ) Local t:TVec3=New TVec3 If IsNan(y) y=x If IsNan(z) z=y t.x=x t.y=y t.z=z Return t EndFunction Function Vec4:TVec4( x#=0.0,y#=0.0/0.0,z#=0.0/0.0,w#=0.0/0.0 ) Local t:TVec4=New TVec4 If IsNan(y) y=x If IsNan(z) z=y If IsNan(w) w=z t.x=x t.y=y t.z=z t.w=w Return t EndFunction Function Line:TLine( x#,y#,z#,dx#,dy#,dz# ) Local t:TLine=New TLine t.x=x t.y=y t.z=z t.dx=dx t.dy=dy t.dz=dz Return t EndFunction Function Quat:TQuat( x#,y#,z#,w# ) Local t:TQuat=New TQuat t.x=x t.y=y t.z=z t.w=w Return t EndFunction Function Plane:TPlane( nx#,ny#,nz#,d# ) Local t:TPlane=New TPlane t.nx=nx t.ny=ny t.nz=nz t.d=d Return t EndFunction Function UlpsEqual( x#,y#,maxUlps=8 ) Return DeltaUlps( x,y )<=maxUlps EndFunction Function DeltaUlps( x#,y# ) Local ix=(Int Ptr Varptr x)[0] Local iy=(Int Ptr Varptr y)[0] If ix<0 ix=$80000000-ix If iy<0 iy=$80000000-iy Return Abs( ix-iy ) EndFunction Function PointDistance#(p1:TVec3,p2:TVec3) Local dx#,dy#,dz# dx=p1.x-p2.x dy=p1.y-p2.y dz=p1.z-p2.z Return Sqr(dx*dx+dy*dy+dz*dz) EndFunction Function PointDistanceSqrd#(p1:TVec3,p2:TVec3) Local dx#,dy#,dz# dx=p1.x-p2.x dy=p1.y-p2.y dz=p1.z-p2.z Return dx*dx+dy*dy+dz*dz EndFunction Type TVec2 Field x#,y# Method ToString$() Return "Vec3("+x+","+y+")" End Method EndType Type TVec3 {value} Field x#,y#,z# {attribute} Method Copy:TVec3() Return Vec3( x,y,z ) End Method Method ToString$() Return "Vec3("+x+","+y+","+z+")" End Method Method ToVec4:TVec4() Return Vec4( x,y,z,1 ) End Method Method ToField$() Return x+","+y+","+z End Method Method FromField:TVec3( t$ ) Local bits$[]=t.Split( "," ) If bits.length<>3 Throw "Format error" x=bits[0].ToFloat() y=bits[1].ToFloat() z=bits[2].ToFloat() Return Self End Method Method Pointer:Float Ptr() Return Varptr x EndMethod Method Length#() Return Sqr( x*x+y*y+z*z ) End Method Method Dot#( v:TVec3 ) Return x*v.x+y*v.y+z*v.z End Method Method Inverse:TVec3() Return Vec3( -x,-y,-z ) End Method Method Reciprocal:TVec3() Return Vec3( 1/x,1/y,1/z ) End Method Method Normalize:TVec3() Local t#=Sqr( x*x+y*y+z*z ) If t=0.0 Return vec3(0.0,0.0,0.0) Return Vec3( x/t,y/t,z/t ) End Method Method Scale:TVec3( scale# ) Return Vec3( x*scale,y*scale,z*scale ) End Method Method DistanceTo#( v:TVec3 ) Local dx#=x-v.x,dy#=y-v.y,dz#=z-v.z Return Sqr( dx*dx+dy*dy+dz*dz ) End Method Method Plus:TVec3( v:TVec3 ) Return Vec3( x+v.x,y+v.y,z+v.z ) End Method Method Minus:TVec3( v:TVec3 ) Return Vec3( x-v.x,y-v.y,z-v.z ) End Method Method Times:TVec3( v:TVec3 ) Return Vec3( x*v.x,y*v.y,z*v.z ) End Method Method DividedBy:TVec3( v:TVec3 ) Return Vec3( x/v.x,y/v.y,z/v.z ) End Method Method Cross:TVec3( v:TVec3 ) Return Vec3( y*v.z-z*v.y,z*v.x-x*v.z,x*v.y-y*v.x ) End Method Method Blend:TVec3( v:TVec3,Alpha# ) Local beta#=1-Alpha Return Vec3( x*beta+v.x*Alpha,y*beta+v.y*Alpha,z*beta+v.z*Alpha ) End Method Method ToQuat:TQuat() Local c1#=Cos(-z/2) Local s1#=Sin(-z/2) Local c2#=Cos(-x/2) Local s2#=Sin(-x/2) Local c3#=Cos( y/2) Local s3#=Sin( y/2) Local c1_c2#=c1*c2 Local s1_s2#=s1*s2 Local t:TQuat=New TQuat t.x=c1*s2*c3 - s1*c2*s3; t.y=c1_c2*s3 + s1_s2*c3; t.z=s1*c2*c3 + c1*s2*s3; t.w=c1_c2*c3 - s1_s2*s3; Return t EndMethod Function FromQuat:TVec3(quat:TQuat) Return quat.toeuler() EndFunction Method Compare:Int(o:Object) Local t:TVec3=TVec3(o) If x<t.x Return -1 If y<t.y Return -1 If z<t.z Return -1 If x>t.x Return 1 If y>t.y Return 1 If z>t.z Return 1 Return 0 EndMethod EndType Type TVec4' Extends TVec3 Field x#,y#,z#,w# {attribute} Method Copy:TVec4() Return Vec4( x,y,z,w ) End Method Method ToString$() Return "Vec4("+x+","+y+","+z+","+w+")" End Method Method ToVec3:TVec3() Return vec3(x,y,z) End Method Method XYZ:TVec3() Return vec3(x,y,z) EndMethod Method Plus:TVec4( v:TVec4 ) Return Vec4( x+v.x,y+v.y,z+v.z,w+v.w ) End Method Method Times:TVec4( v:TVec4 ) Return Vec4( x*v.x,y*v.y,z*v.z,w*v.w ) End Method Method Pointer:Float Ptr() Return Varptr x EndMethod EndType Type TLine Field x#,y#,z#,dx#,dy#,dz# {attribute} Method Copy:TLine() Return Line( x,y,z,dx,dy,dz ) End Method Method ToString$() Return "Line("+x+","+y+","+z+","+dx+","+dy+","+dz+")" End Method Method Origin:TVec3() Return Vec3( x,y,z ) End Method Method Delta:TVec3() Return Vec3( dx,dy,dz ) End Method Method EndPoint:TVec3() Return Vec3( x+dx,y+dy,z+dz ) End Method Method Evaluate:TVec3( time# ) Return Vec3( x+dx*time,y+dy*time,z+dz*time ) End Method Method DistanceTo#( t:TLine ) Local wx#=x-t.x Local wy#=y-t.y Local wz#=z-t.z Local a#=dx*dx+dy*dy+dz*dz Local b#=dx*t.dx+dy*t.dy+dz*t.dz Local c#=t.dx*t.dx+t.dy*t.dy+t.dz*t.dz Local d#=dx*wx+dy*wy+dz*wz Local e#=t.dx*wx+t.dy*wy+t.dz*wz Local q#=a*c-b*b Local st#=b*e-c*d Local tt#=a*e-b*d Local tx#=wx+(st*dx-tt*t.dx)/q Local ty#=wy+(st*dy-tt*t.dy)/q Local tz#=wz+(st*dz-tt*t.dz)/q Return Sqr( tx*tx+ty*ty+tz*tz ) End Method Method Intersects:TVec3( t:TLine,tolerance#=0.001 ) Local wx#=x-t.x Local wy#=y-t.y Local wz#=z-t.z Local a#=dx*dx+dy*dy+dz*dz Local b#=dx*t.dx+dy*t.dy+dz*t.dz Local c#=t.dx*t.dx+t.dy*t.dy+t.dz*t.dz Local d#=dx*wx+dy*wy+dz*wz Local e#=t.dx*wx+t.dy*wy+t.dz*wz Local q#=a*c-b*b Local st#=b*e-c*d Local tt#=a*e-b*d Local tx#=wx+(st*dx-tt*t.dx)/q Local ty#=wy+(st*dy-tt*t.dy)/q Local tz#=wz+(st*dz-tt*t.dz)/q If tolerance<>-1 If tx*tx+ty*ty+tz*tz>=tolerance*tolerance Return EndIf tx#=wx+(st*dx)/q ty#=wy+(st*dy)/q tz#=wz+(st*dz)/q Return vec3(tx,ty,tz).plus(vec3(t.x,t.y,t.z)) EndMethod Function FromEndPoints:TLine( p0:TVec3,p1:TVec3 ) Local t:TLine=New TLine t.x=p0.x;t.y=p0.y;t.z=p0.z t.dx=p1.x-p0.x;t.dy=p1.y-p0.y;t.dz=p1.z-p0.z; Return t EndFunction EndType Type TQuat Field x#,y#,z#,w#=1.0 {attribute} Method Copy:TQuat() Return Quat( x,y,z,w ) End Method Method ToString$() Return "Quat("+x+","+y+","+z+","+w+")" End Method Method Inverse:TQuat() Return Quat( -x,-y,-z,w ) End Method Method ToAngleAxis( angle# Var,axis:TVec3 Var ) Local t#=ACos(w) angle=t*2 If angle>0 t=1/Sin(t) axis=Vec3( x*t,y*t,z*t ) Else axis=Vec3(0,0,0) EndIf End Method Method Yaw#() Return ATan2( 2*y*w-2*z*x,1-2*y*y-2*x*x ) End Method Method Pitch#() Return -ASin( 2*z*y+2*x*w ) End Method Method Roll#() Return -ATan2( 2*z*w-2*y*x,1-2*z*z-2*x*x ) End Method Method ToEuler:TVec3() Local x2#=x*x,y2#=y*y,z2#=z*z Local euler:TVec3=New TVec3 euler.y#=ATan2( 2*y*w-2*z*x,1-2*y2-2*x2 ) euler.x#=-ASin( 2*z*y+2*x*w ) euler.z#=-ATan2( 2*z*w-2*y*x,1-2*z2-2*x2 ) Return euler End Method Method Times:TQuat( q:TQuat ) Local result:TQuat=New TQuat result.x#=w*q.x + x*q.w + y*q.z - z*q.y result.y#=w*q.y + y*q.w + z*q.x - x*q.z result.z#=w*q.z + z*q.w + x*q.y - y*q.x result.w#=w*q.w - x*q.x - y*q.y - z*q.z Return result EndMethod Method Slerp:TQuat( q:TQuat,a# ) Local b#=1-a,f Local d#=x*q.x+y*q.y+z*q.z+w*q.w If d<0 d=-d f=True EndIf If d<1 Local om#=ACos( d )'- Local si#=Sin( om )'- a=Sin( a*om )/si'- b=Sin( b*om )/si'- EndIf If f a=-a Return Quat( x*b + q.x*a , y*b + q.y*a , z*b + q.z*a , w*b + q.w*a ) EndMethod Method Normalize:TQuat() Local q:TQuat=New TQuat Local m#=Sqr(x*x+y*y+z*z+w*w) q.x=x/m q.y=y/m q.z=z/m q.w=w/m Return q EndMethod Function FromAngleAxis:TQuat( angle#,axis:TVec3 ) angle:/2 Local s#=Sin(angle) Local t:TQuat=New TQuat t.x=s*axis.x t.y=s*axis.y t.z=s*axis.z t.w=Cos(angle) Return t EndFunction Function FromEuler:TQuat(euler:TVec3) Return euler.toquat() EndFunction EndType Type TPlane Field nx#,ny#,nz#,d# {attribute} Method Copy:TPlane() Return Plane( nx,ny,nz,d ) End Method Method ToString$() Return "Plane("+nx+","+ny+","+nz+","+d+")" End Method Method MajorAxis() Local ax#=Abs(nx),ay#=Abs(ny),az#=Abs(nz) If ax>ay And ax>az Return 0 If ay>az Return 1 Return 2 End Method Method SolveX:TVec3( p:TVec3 ) Return Vec3( (p.y*ny+p.z*nz+d)/-nx,p.y,p.z ) End Method Method SolveY:TVec3( p:TVec3 ) Return Vec3( p.x,(p.x*nx+p.z*nz+d)/-ny,p.z ) End Method Method SolveZ:TVec3( p:TVec3 ) Return Vec3( p.x,p.y,(p.x*nx+p.y*ny+d)/-nz ) End Method Method Inverse:TPlane() Return Plane( -nx,-ny,-nz,-d ) End Method Method Normal:TVec3() Return Vec3( nx,ny,nz ) End Method Method Normalize:TPlane() Local i#=1/Sqr(nx*nx+ny*ny+nz*nz) Return Plane( nx*i,ny*i,nz*i,d*i ) End Method Method t_IntersectLine#( t:TLine ) Return -DistanceToPoint( t.Origin() ) / Normal().Dot( t.Delta() ) End Method Method IntersectsLine:TVec3(l:TLine) Local v:TVec3=New TVec3 Local u# u#=(nx*l.x+ny*l.y+nz*l.z+d)/(nx*l.dx+ny*l.dy+nz*l.dz) v.x=l.x-l.dx*u v.y=l.y-l.dy*u v.z=l.z-l.dz*u Return v EndMethod Method DistanceToPoint#( p:TVec3 ) Return nx*p.x+ny*p.y+nz*p.z+d End Method Function FromPointNormal:TPlane( p:TVec3,n:TVec3 ) Return Plane( n.x,n.y,n.z,-p.Dot(n) ) EndFunction Function FromTriangle:TPlane( v0:TVec3,v1:TVec3,v2:TVec3 ) Local va:TVec3=v2.Minus(v1) Local vb:TVec3=v0.Minus(v1) Return FromPointNormal( v1,va.Cross(vb).Normalize() ) EndFunction Function Ground:TPlane() Return Plane( 0,1,0,0 ) EndFunction EndType Type TMat4 Field ix#,iy#,iz#,iw# {attribute} Field jx#,jy#,jz#,jw# {attribute} Field kx#,ky#,kz#,kw# {attribute} Field tx#,ty#,tz#,tw# {attribute} Method Copy:TMat4() Local t:TMat4=New TMat4 MemCopy t,Self,SizeOf Self Return t End Method Method ToString$() Local t$="Mat4{~n" t:+ix+","+iy+","+iz+","+iw+"~n" t:+jx+","+jy+","+jz+","+jw+"~n" t:+kx+","+ky+","+kz+","+kw+"~n" t:+tx+","+ty+","+tz+","+tw+"~n" Return t+"}~n" End Method Method Pointer:Float Ptr() Return Varptr ix EndMethod Method Translation:TVec3() Return Vec3(tx,ty,tz) End Method Rem Local sc:TVec3=Scale() Local iv:TVec3=Vec3(ix,iy,iz).Scale(1/sc.x) Local jv:TVec3=Vec3(jx,jy,jz).Scale(1/sc.y) Local kv:TVec3=Vec3(kx,ky,kz).Scale(1/sc.z) Local x#,y#,z#,w# Local t#=iv.x+jv.y+kv.z+1 If t>0 t=Sqr(t)*2 x=(kv.y-jv.z)/t y=(iv.z-kv.x)/t z=(jv.x-iv.y)/t w=t/4 Else If iv.x>jv.y And iv.x>kv.z t=Sqr(iv.x-jv.y-kv.z+1)*2.0 x=t/4 y=(jv.x+iv.y)/t z=(iv.z+kv.x)/t w=(kv.y-jv.z)/t Else If jv.y>kv.z t=Sqr(jv.y-kv.z-iv.x+1)*2.0 x=(jv.x+iv.y)/t y=t/4 z=(kv.y+jv.z)/t w=(iv.z-kv.x)/t Else t=Sqr(kv.z-jv.y-iv.x+1)*2.0 x=(iv.z+kv.x)/t y=(kv.y+jv.z)/t z=t/4 w=(jv.x-iv.y)/t EndIf Return Quat( x,y,z,w ) EndRem Method Rotation:TQuat() Return normalize().Rotation2() EndMethod Method Rotation2:TQuat() Local mult:Float Local x#,y#,z#,w# Local fourWSquaredMinus1:Float = +ix+jy+kz Local fourXSquaredMinus1:Float = +ix-jy-kz Local fourYSquaredMinus1:Float = +jy-ix-kz Local fourZSquaredMinus1:Float = +kz-ix-jy Local biggestIndex=0 Local fourBiggestSquaredMinus1:Float=fourWSquaredMinus1 If fourXSquaredMinus1 > fourBiggestSquaredMinus1 fourBiggestSquaredMinus1 = fourXSquaredMinus1 biggestIndex=1 EndIf If fourYSquaredMinus1 > fourBiggestSquaredMinus1 fourBiggestSquaredMinus1 = fourYSquaredMinus1 biggestIndex=2 EndIf If fourZSquaredMinus1 > fourBiggestSquaredMinus1 fourBiggestSquaredMinus1 = fourZSquaredMinus1 biggestIndex=3 EndIf Local biggestValue:Float=Sqr(fourBiggestSquaredMinus1+1.0)*0.5 mult=0.25/biggestValue Select BiggestIndex Case 0 w#=BiggestValue x#=(jz-ky)*mult y#=-(kx-iz)*mult z#=-(iy-jx)*mult Case 1 x#=-BiggestValue w#=-( jz - ky )*mult y#=( iy + jx )*mult z#=( kx + iz )*mult Case 2 y#=BiggestValue w#=-(kx-iz)*mult x#=-(iy+jx)*mult z#=(jz+ky)*mult Case 3 z#=-BiggestValue w#=(iy-jx)*mult x#=(kx+iz)*mult y#=-(jz+ky)*mult EndSelect Return quat(-x,y,z,w) EndMethod Function KeyHit(blah) EndFunction Rem 'testing code: Global mode=0 If KeyHit(KEY_N) mode:+1 'Notify mode EndIf If KeyHit(KEY_L) Notify mode Select mode Case 0 Return quat(x,y,z,w) Case 1 Return quat(x,y,z,-w) Case 2 Return quat(x,-y,z,w) Case 3 Return quat(x,y,-z,w) Case 4 Return quat(-x,-y,z,w) Case 5 Return quat(x,-y,-z,w) Case 6 Return quat(-x,y,z,w) EndSelect EndRem Method Scale:TVec3() Return Vec3( Vec3(ix,iy,iz).Length(),Vec3(jx,jy,jz).Length(),Vec3(kx,ky,kz).Length() ) End Method Method Times:TMat4( m:TMat4 ) Local t:TMat4=New TMat4 t.ix= ix*m.ix + jx*m.iy + kx*m.iz + tx*m.iw t.iy= iy*m.ix + jy*m.iy + ky*m.iz + ty*m.iw t.iz= iz*m.ix + jz*m.iy + kz*m.iz + tz*m.iw t.iw= iw*m.ix + jw*m.iy + kw*m.iz + tw*m.iw t.jx= ix*m.jx + jx*m.jy + kx*m.jz + tx*m.jw t.jy= iy*m.jx + jy*m.jy + ky*m.jz + ty*m.jw t.jz= iz*m.jx + jz*m.jy + kz*m.jz + tz*m.jw t.jw= iw*m.jx + jw*m.jy + kw*m.jz + tw*m.jw t.kx= ix*m.kx + jx*m.ky + kx*m.kz + tx*m.kw t.ky= iy*m.kx + jy*m.ky + ky*m.kz + ty*m.kw t.kz= iz*m.kx + jz*m.ky + kz*m.kz + tz*m.kw t.kw= iw*m.kx + jw*m.ky + kw*m.kz + tw*m.kw t.tx= ix*m.tx + jx*m.ty + kx*m.tz + tx*m.tw t.ty= iy*m.tx + jy*m.ty + ky*m.tz + ty*m.tw t.tz= iz*m.tx + jz*m.ty + kz*m.tz + tz*m.tw t.tw= iw*m.tx + jw*m.ty + kw*m.tz + tw*m.tw Return t End Method Method TimesPoint:TVec3( v:TVec3 ) ' Rem Return Vec3(.. ix*v.x + jx*v.y + kx*v.z + tx,.. iy*v.x + jy*v.y + ky*v.z + ty,.. iz*v.x + jz*v.y + kz*v.z + tz ) ' End Rem ' Local x#=ix*v.x + jx*v.y + kx*v.z + tx ' Local y#=iy*v.x + jy*v.y + ky*v.z + ty ' Local z#=iz*v.x + jz*v.y + kz*v.z + tz ' Local w#=iw*v.x + jw*v.y + kw*v.z + tw ' Return Vec3( x/w,y/w,z/w ) End Method Method TimesVector:TVec3( v:TVec3 ) Return Vec3(.. ix*v.x + jx*v.y + kx*v.z,.. iy*v.x + jy*v.y + ky*v.z,.. iz*v.x + jz*v.y + kz*v.z ) End Method Method TimesNormal:TVec3( v:TVec3 ) Local m:TMat4=Inverse() Return Vec3(.. m.ix*v.x + m.iy*v.y + m.iz*v.z,.. m.jx*v.x + m.jy*v.y + m.jz*v.z,.. m.kx*v.x + m.ky*v.y + m.kz*v.z ) End Method Method TimesPlane:TPlane( p:TPlane ) Local m:TMat4=Inverse() Return Plane(.. m.ix*p.nx + m.iy*p.ny + m.iz*p.nz + m.iw*p.d,.. m.jx*p.nx + m.jy*p.ny + m.jz*p.nz + m.jw*p.d,.. m.kx*p.nx + m.ky*p.ny + m.kz*p.nz + m.kw*p.d,.. m.tx*p.nx + m.ty*p.ny + m.tz*p.nz + m.tw*p.d ).Normalize() End Method Method TimesLine:TLine( t:TLine ) Return TLine.FromEndPoints( TimesPoint( t.Origin() ),TimesPoint( t.EndPoint() ) ) End Method Method Determinant#() Assert Abs(iw)<=.001 And Abs(jw)<=.001 And Abs(kw<=.001) And tw>=1-.001 Return ix*(jy*kz-jz*ky) - iy*(jx*kz-jz*kx) + iz*(jx*ky-jy*kx) End Method Method I:TVec3() Return Vec3(ix,iy,iz) End Method Method J:TVec3() Return Vec3(jx,jy,jz) End Method Method K:TVec3() Return Vec3(kx,ky,kz) End Method Method Cofactor:TMat4() Local t:TMat4=New TMat4 t.ix= (jy*kz-jz*ky) ; t.iy=-(jx*kz-jz*kx) ; t.iz= (jx*ky-jy*kx) t.jx=-(iy*kz-iz*ky) ; t.jy= (ix*kz-iz*kx) ; t.jz=-(ix*ky-iy*kx) t.kx= (iy*jz-iz*jy) ; t.ky=-(ix*jz-iz*jx) ; t.kz= (ix*jy-iy*jx) Return t End Method Method Inverse:TMat4() Local c#=1.0/Determinant() Local t:TMat4=New TMat4 t.ix= c * ( jy*kz - jz*ky ) t.iy=-c * ( iy*kz - iz*ky ) t.iz= c * ( iy*jz - iz*jy ) t.jx=-c * ( jx*kz - jz*kx ) t.jy= c * ( ix*kz - iz*kx ) t.jz=-c * ( ix*jz - iz*jx ) t.kx= c * ( jx*ky - jy*kx ) t.ky=-c * ( ix*ky - iy*kx ) t.kz= c * ( ix*jy - iy*jx ) t.tx=-( tx*t.ix + ty*t.jx + tz*t.kx ) t.ty=-( tx*t.iy + ty*t.jy + tz*t.ky ) t.tz=-( tx*t.iz + ty*t.jz + tz*t.kz ) t.tw=1 Return t End Method Method MyInverse:TMat4() Local t:TMat4=Transpose() t.iw=0 t.jw=0 t.kw=0 t.tw=1 t.tx=-( tx*t.ix + ty*t.jx + tz*t.kx ) t.ty=-( tx*t.iy + ty*t.jy + tz*t.ky ) t.tz=-( tx*t.iz + ty*t.jz + tz*t.kz ) Return t 'mat.tx = -( (ix * tx) + (grid[0,1] * ty) + (grid[0,2] * tz) ) 'mat.ty = -( (jx * tx) + (grid[1,1] * ty) + (grid[1,2] * tz) ) 'mat.tz = -( (kx * tx) + (grid[2,1] * ty) + (grid[2,2] * tz) ) EndMethod Method Transpose:TMat4() Local t:TMat4=New TMat4 t.ix=ix ; t.iy=jx ; t.iz=kx ; t.iw=tx t.jx=iy ; t.jy=jy ; t.jz=ky ; t.jw=ty t.kx=iz ; t.ky=jz ; t.kz=kz ; t.kw=tz t.tx=iw ; t.ty=jw ; t.tz=kw ; t.tw=tw Return t End Method Function FromDirection:TMat4( dir:TVec3,up:TVec3=Null ) If Not up up=Vec3(0,1,0) Local j:TVec3=up.Normalize() Local k:TVec3=dir.Normalize() Local i:TVec3=j.Cross(k).Normalize() j=k.Cross(i).Normalize() Local t:TMat4=New TMat4 t.ix=i.x ; t.iy=i.y ; t.iz=i.z t.jx=j.x ; t.jy=j.y ; t.jz=j.z t.kx=k.x ; t.ky=k.y ; t.kz=k.z t.tw=1 Return t EndFunction Function FromTranslation:TMat4( v:TVec3 ) Local t:TMat4=New TMat4 t.ix=1 ; t.jy=1 ; t.kz=1 t.tx=v.x ; t.ty=v.y ; t.tz=v.z t.tw=1 Return t EndFunction Function FromRotation:TMat4( q:TQuat ) Local t:TMat4=New TMat4 Local xx#=q.x*q.x,yy#=q.y*q.y,zz#=q.z*q.z Local xy#=q.x*q.y,xz#=q.x*q.z,yz#=q.y*q.z Local wx#=q.w*q.x,wy#=q.w*q.y,wz#=q.w*q.z t.ix=1-2*(yy+zz) ; t.iy= 2*(xy-wz) ; t.iz= 2*(xz+wy) t.jx= 2*(xy+wz) ; t.jy=1.0-2*(xx+zz) ; t.jz= 2*(yz-wx) t.kx= 2*(xz-wy) ; t.ky= 2*(yz+wx) ; t.kz=1.0-2*(xx+yy) t.tw=1 Return t EndFunction Function FromScale:TMat4( v:TVec3 ) Local t:TMat4=New TMat4 t.ix=v.x t.jy=v.y t.kz=v.z t.tw=1 Return t EndFunction Function FromUniformScale:TMat4( sc# ) Local t:TMat4=New TMat4 t.ix=sc t.jy=sc t.kz=sc t.tw=1 Return t EndFunction Function FromTransRotScale:TMat4( trans:TVec3,rot:TQuat,scale:TVec3 ) Local t:TMat4 If trans t=FromTranslation(trans) If rot t=t.Times(FromRotation(rot)) If scale t=t.Times(FromScale(scale)) Else If rot t=FromRotation(rot) If scale t=t.Times(FromScale(scale)) Else If scale t=FromScale(scale) Else t=Identity() EndIf Return t EndFunction Function FromYaw:TMat4( yaw# ) 'untested! Local t:TMat4=New TMat4 Local s#=Sin(yaw),c#=Cos(yaw) t.ix=c ; t.iz=s ; t.jy=1 t.kx=-s ; t.kz=c ; t.tw=1 Return t EndFunction Function FromPitch:TMat4( pitch# ) 'untested! Local t:TMat4=New TMat4 Local s#=Sin(pitch),c#=Cos(pitch) t.ix=1 ; t.jy=c ; t.jz=s t.ky=-s ; t.kz=c ; t.tw=1 Return t EndFunction Function FromRoll:TMat4( roll# ) 'untested! Local t:TMat4=New TMat4 Local s#=Sin(roll),c#=Cos(roll) t.ix=c ; t.iy=s ; t.jx=-s t.jy=c ; t.kz=1 ; t.tw=1 Return t EndFunction Function FromYawPitchRoll:TMat4( yaw#,pitch#,roll# ) Local t:TMat4 If yaw t=FromYaw(yaw) If pitch t=t.Times(FromPitch(pitch)) If roll t=t.Times(FromRoll(roll)) Else If pitch t=FromPitch(pitch) If roll t=t.Times(FromRoll(roll)) Else If roll t=FromRoll(roll) Else t=Identity() EndIf Return t EndFunction Function FromOrtho:TMat4( l#,r#,b#,t#,n#,f# ) Local q:TMat4=New TMat4 q.ix=2/(r-l) q.tx=-(r+l)/(r-l) q.jy=2/(t-b) q.ty=-(t+b)/(t-b) q.kz=2/(f-n) q.tz=-(f+n)/(f-n) q.tw=1 Return q EndFunction Function FromFrustum:TMat4( near_left#,near_right#,near_bottom#,near_top#,near#,far# ) Local t:TMat4=New TMat4 Local near2#=near*2 Local w#=near_right-near_left Local h#=near_top-near_bottom Local d#=far-near t.ix=near2/w t.jy=near2/h t.kx=(near_right+near_left)/w t.ky=(near_top+near_bottom)/h t.kz=(far+near)/d t.kw=1 t.tz=-(far*near2)/d Return t EndFunction Function FromInfiniteFrustum:TMat4( near_left#,near_right#,near_bottom#,near_top#,near# ) Local t:TMat4=New TMat4 Local near2#=near*2 Local w#=near_right-near_left Local h#=near_top-near_bottom Local e#=.001 t.ix=near2/w t.jy=near2/w t.kx=(near_right+near_left)/w t.ky=(near_top+near_bottom)/h t.kz=1-e t.kw=1 t.tz=near*(e-1) Return t EndFunction Function Identity:TMat4() Local t:TMat4=New TMat4 t.ix=1 ; t.jy=1 ; t.kz=1 ; t.tw=1 Return t EndFunction Method Normalize:TMat4() Local mat:TMat4=New TMat4 Local vec:TVec3 vec=i().normalize() mat.ix=vec.x mat.iy=vec.y mat.iz=vec.z vec=j().normalize() mat.jx=vec.x mat.jy=vec.y mat.jz=vec.z vec=k().normalize() mat.kx=vec.x mat.ky=vec.y mat.kz=vec.z mat.tx=tx mat.ty=ty mat.tz=tz mat.tw=1.0 Return mat EndMethod EndType Type TAABB Field x0#,y0#,z0#,x1#,y1#,z1#,cx#,cy#,cz#,w#,h#,d#,radius# { attribute } Method Reset() x0=0.0 y0=0.0 z0=0.0 x1=0.0 y1=0.0 z1=0.0 cx#=0.0 cy#=0.0 cz#=0.0 w#=0.0 h#=0.0 d#=0.0 EndMethod 'Rem Method TForm:TAABB(src:TMat4,dst:TMat4) Local vec:TVec3 Local aabb:TAABB aabb=New taabb vec=TFormPointm(vec3(x0,y0,z0),src,dst) aabb.x0=vec.x aabb.y0=vec.y aabb.z0=vec.z aabb.x1=vec.x aabb.y1=vec.y aabb.z1=vec.z vec=TFormPointm(vec3(x0,y0,z1),src,dst) aabb.x0=Min(aabb.x0,vec.x) aabb.y0=Min(aabb.y0,vec.y) aabb.z0=Min(aabb.z0,vec.z) aabb.x1=Max(aabb.x1,vec.x) aabb.y1=Max(aabb.y1,vec.y) aabb.z1=Max(aabb.z1,vec.z) vec=TFormPointm(vec3(x0,y1,z0),src,dst) aabb.x0=Min(aabb.x0,vec.x) aabb.y0=Min(aabb.y0,vec.y) aabb.z0=Min(aabb.z0,vec.z) aabb.x1=Max(aabb.x1,vec.x) aabb.y1=Max(aabb.y1,vec.y) aabb.z1=Max(aabb.z1,vec.z) vec=TFormPointm(vec3(x1,y0,z0),src,dst) aabb.x0=Min(aabb.x0,vec.x) aabb.y0=Min(aabb.y0,vec.y) aabb.z0=Min(aabb.z0,vec.z) aabb.x1=Max(aabb.x1,vec.x) aabb.y1=Max(aabb.y1,vec.y) aabb.z1=Max(aabb.z1,vec.z) vec=TFormPointm(vec3(x1,y1,z0),src,dst) aabb.x0=Min(aabb.x0,vec.x) aabb.y0=Min(aabb.y0,vec.y) aabb.z0=Min(aabb.z0,vec.z) aabb.x1=Max(aabb.x1,vec.x) aabb.y1=Max(aabb.y1,vec.y) aabb.z1=Max(aabb.z1,vec.z) vec=TFormPointm(vec3(x0,y1,z1),src,dst) aabb.x0=Min(aabb.x0,vec.x) aabb.y0=Min(aabb.y0,vec.y) aabb.z0=Min(aabb.z0,vec.z) aabb.x1=Max(aabb.x1,vec.x) aabb.y1=Max(aabb.y1,vec.y) aabb.z1=Max(aabb.z1,vec.z) vec=TFormPointm(vec3(x1,y0,z1),src,dst) aabb.x0=Min(aabb.x0,vec.x) aabb.y0=Min(aabb.y0,vec.y) aabb.z0=Min(aabb.z0,vec.z) aabb.x1=Max(aabb.x1,vec.x) aabb.y1=Max(aabb.y1,vec.y) aabb.z1=Max(aabb.z1,vec.z) vec=TFormPointm(vec3(x1,y1,z1),src,dst) aabb.x0=Min(aabb.x0,vec.x) aabb.y0=Min(aabb.y0,vec.y) aabb.z0=Min(aabb.z0,vec.z) aabb.x1=Max(aabb.x1,vec.x) aabb.y1=Max(aabb.y1,vec.y) aabb.z1=Max(aabb.z1,vec.z) aabb.Update() Return aabb EndMethod Method Update() w=x1-x0 h=y1-y0 d=z1-z0 cx=x0+w*0.5 cy=y0+h*0.5 cz=z0+d*0.5 UpdateRadius() EndMethod Method IntersectsPoint:Int(p:TVec3,radius#=0.0) If p.x<x0-radius Return False If p.y<y0-radius Return False If p.z<z0-radius Return False If p.x>x1+radius Return False If p.y>y1+radius Return False If p.z>z1+radius Return False Return True EndMethod Method IntersectsAABB:Int(aabb:TAABB) If aabb.x0>x1 Return False If aabb.y0>y1 Return False If aabb.z0>z1 Return False If aabb.x1<x0 Return False If aabb.y1<y0 Return False If aabb.z1<z0 Return False Return True EndMethod Function FromCloud:TAABB(vertex_count,vertex_cloud:Float Ptr) Local aabb:TAABB=New TAABB Local v:Int For v=0 To vertex_count-1 If v=0 Or vertex_cloud[v*3+0]<aabb.x0 aabb.x0=vertex_cloud[v*3+0] If v=0 Or vertex_cloud[v*3+1]<aabb.y0 aabb.y0=vertex_cloud[v*3+1] If v=0 Or vertex_cloud[v*3+2]<aabb.z0 aabb.z0=vertex_cloud[v*3+2] If v=0 Or vertex_cloud[v*3+0]>aabb.x1 aabb.x1=vertex_cloud[v*3+0] If v=0 Or vertex_cloud[v*3+1]>aabb.y1 aabb.y1=vertex_cloud[v*3+1] If v=0 Or vertex_cloud[v*3+2]>aabb.z1 aabb.z1=vertex_cloud[v*3+2] Next aabb.Update() Return aabb EndFunction Method UpdateRadius() Local dx#,dy#,dz# dx=w*0.5 dy=h*0.5 dz=d*0.5 radius=Sqr(dx*dx+dy*dy+dz*dz) EndMethod Method MinExtent:TVec3() Return vec3(x0,y0,z0) EndMethod Method MaxExtent:TVec3() Return vec3(x1,y1,z1) EndMethod Method Center:TVec3() Return vec3(cx,cy,cz) EndMethod Method Copy:TAABB() Local aabb:TAABB aabb=New TAABB aabb.x0=x0 aabb.y0=y0 aabb.z0=z0 aabb.x1=x1 aabb.y1=y1 aabb.z1=z1 aabb.cx=cx aabb.cy=cy aabb.cz=cz aabb.w=w aabb.h=h aabb.d=d aabb.radius=radius Return aabb EndMethod Method IntersectsPlane:Int(plane:TPlane) Local d#,n Local greater Local lesser Local p:TVec3 For n=1 To 8 Select n Case 1 p=vec3(x0,y0,z0) Case 2 p=vec3(x1,y0,z0) Case 3 p=vec3(x0,y1,z0) Case 4 p=vec3(x0,y0,z1) Case 5 p=vec3(x1,y1,z1) Case 6 p=vec3(x0,y1,z1) Case 7 p=vec3(x1,y0,z1) Case 8 p=vec3(x1,y1,z1) EndSelect d#=plane.DistanceToPoint(p) If d>0.0 greater=True If d<0.0 lesser=True If greater=True And lesser=True Return 0 Next If greater=True Return 1 If lesser=True Return -1 EndMethod Method ToString$() Return x0+", "+y0+", "+z0+", "+x1+", "+y1+", "+z1+Chr(13)+Chr(10)+cx+", "+cy+", "+cz EndMethod Method Intersects:Int(aabb:TAABB,bias#=0.0) If x0>aabb.x1-bias Return If y0>aabb.y1-bias Return If z0>aabb.z1-bias Return If aabb.x0>x1-bias Return If aabb.y0>y1-bias Return If aabb.z0>z1-bias Return Return 1 EndMethod EndType Function TFormPointm:TVec3(p:TVec3,mat1:TMat4,mat2:TMat4) Local mat:TMat4 If mat1 mat=mat1.times(TMat4.FromTranslation(p)) p=mat.Translation() EndIf If mat2 mat=mat2.inverse().times(TMat4.FromTranslation(p)) p=mat.Translation() EndIf If mat1=Null And mat2=Null Return p.copy() Else Return p EndFunction Function TFormVectorm:TVec3(p:TVec3,mat1:TMat4,mat2:TMat4) Local mat:TMat4 If mat1 mat=mat1.copy() mat.tx=0.0 ; mat.ty=0.0 ; mat.tz=0.0 mat=mat.times(TMat4.FromTranslation(p)) p=mat.Translation() EndIf If mat2 mat=mat2.inverse() mat.tx=0.0 ; mat.ty=0.0 ; mat.tz=0.0 mat=mat.times(TMat4.FromTranslation(p)) p=mat.Translation() EndIf If mat1=Null And mat2=Null Return p.copy() Else Return p EndFunction Function TFormNormalm:TVec3(p:TVec3,mat1:TMat4,mat2:TMat4) Return TFormVectorm(p,mat1,mat2).normalize() EndFunction Function TFormQuatm:TQuat(quat:TQuat,mat1:TMat4,mat2:TMat4) Local mat:TMat4 If mat1 mat=mat1.copy() mat.tx=0.0 ; mat.ty=0.0 ; mat.tz=0.0 mat=mat.times(TMat4.FromRotation(quat)) quat=mat.rotation() EndIf If mat2 mat=mat2.inverse() mat.tx=0.0 ; mat.ty=0.0 ; mat.tz=0.0 mat=mat.times(TMat4.FromRotation(quat)) quat=mat.rotation().normalize() EndIf If mat1=Null And mat2=Null Return quat.copy() Else Return quat EndFunction