;Vektor und Matrizen Funktionssammlung
;by Steffen Nörtershäuser

;----------------------------------------Types----------------------------------------------------------

;Dieser Type verwaltet eine 4x4 Matrix
Type Matrix4x4
 Field Mat#[16]
End Type

Type Vector4
 Field Vector#[3]
End Type

;-------------------------------------------------------------------------------------------------------


;----------------------------------------Funktionen-----------------------------------------------------

;---------------Matrizen------------------------------

Function BuildIdentityMatrix4x4.Matrix4x4()
  ;Diese Funktion erstellt eine EinheitsMatrix und gibt diese zurück

  Local Mat.Matrix4x4 = New Matrix4x4

  For xy = 0 To 3
   SetMatrix4x4Value(Mat,xy,xy,1)
  Next

  Return Mat
End Function

Function BuildTransponseMatrix4x4.Matrix4x4(Mat1.Matrix4x4)
  ;Diese Funktion berechnet die Transponierte Matrix der angeben Matrix und gibt diese zurück

  Local Mat.Matrix4x4 = New Matrix4x4

  For x = 0 To 3
    For y = 0 To 3
     SetMatrix4x4Value(Mat,y,x,GetMatrix4x4Value(Mat1,x,y))
    Next
  Next

  Return Mat
End Function

Function CopyMatrix4x4.Matrix4x4(Mat1.Matrix4x4)
  ;Diese Funktion kopiert die angeben Matrix und gibt die Kopie zurück

  Local Mat.Matrix4x4 = New Matrix4x4

  For x = 0 To 3
    For y = 0 To 3
     SetMatrix4x4Value(Mat,x,y,GetMatrix4x4Value(Mat1,x,y))
    Next
  Next

  Return Mat
End Function

Function GetMatrix4x4Value#(Mat.Matrix4x4, Row, Column)
  ;Diese Funktion gibt den Wert der angeben Matrix an der gegebenen Stelle zurück
  If Row < 0 Or Row > 3 Then Return 0
  If Column < 0 Or Column> 3 Then Return 0

  Return Mat\Mat[Row*4+Column]
End Function

Function SetMatrix4x4Value(Mat.Matrix4x4, Row, Column, Value#)
  ;Diese Funktion gibt den Wert der angeben Matrix an der gegebenen Stelle zurück
  If Row < 0 Or Row > 3 Then Return
  If Column < 0 Or Column > 3 Then Return

  Mat\Mat[Row*4+Column] = Value
End Function

Function AddMatrix4x4.Matrix4x4(Mat1.Matrix4x4, Mat2.Matrix4x4)
  ;Diese Funktion addiert die beiden angeben Matrizen und gibt das Ergebnis zurück
   
  Local Mat.Matrix4x4 = New Matrix4x4

  For x = 0 To 3
    For y = 0 To 3
     SetMatrix4x4Value(Mat,x,y,GetMatrix4x4Value(Mat1,x,y)+GetMatrix4x4Value(Mat2,x,y))
    Next
  Next
 
  Return Mat
End Function

Function AddMatrix4x4Skalar.Matrix4x4(Mat1.Matrix4x4, Value#)
  ;Diese Funktion addiert die angebe Matrix mit dem angebenen Skalar und gibt das Ergebnis zurück
   
  Local Mat.Matrix4x4 = New Matrix4x4

  For x = 0 To 3
    For y = 0 To 3
     SetMatrix4x4Value(Mat,x,y,GetMatrix4x4Value(Mat1,x,y)+Value)
    Next
  Next
 
  Return Mat
End Function

Function MultiplyMatrix4x4.Matrix4x4(Mat1.Matrix4x4, Mat2.Matrix4x4)
  ;Diese Funktion multipliziert die beiden angeben Matrizen miteinander und gibt das Ergebnis zurück
   
  Local Mat.Matrix4x4 = New Matrix4x4
  Local Value# = 0.0
 
  For x = 0 To 3
    For y = 0 To 3

      For x2 = 0 To 3
       Value = Value + GetMatrix4x4Value(Mat1,x2,y)*GetMatrix4x4Value(Mat2,x,x2)
      Next
     
     SetMatrix4x4Value(Mat,x,y,Value)
     Value = 0.0
    Next
  Next

  Return Mat
End Function

Function MultiplyMatrix4x4Skalar.Matrix4x4(Mat1.Matrix4x4, Value#)
  ;Diese Funktion multipliziert die beiden angeben Matrizen und gibt das Ergebnis zurück
   
  Local Mat.Matrix4x4 = New Matrix4x4
 
  For x = 0 To 3
    For y = 0 To 3
     SetMatrix4x4Value(Mat,x,y,GetMatrix4x4Value(Mat1,x2,y)*Value)
    Next
  Next

  Return Mat
End Function

Function ExpMatrix4x4.Matrix4x4(Mat1.Matrix4x4, Expo)
  ;Diese Funktion potenziert die angebe Matrix und gibt das Ergebnis zurück

  Local Mat.Matrix4x4 = CopyMatrix4x4(Mat1)
  Local OldMat.Matrix4x4
  Local Value# = 0.0
 
  For CurExp = 2 To Expo
   OldMat = Mat
    Mat = MultiplyMatrix4x4(Mat,Mat)
    Delete OldMat
  Next

  Return Mat
End Function


;---------------Vektoren------------------------------

Function GetVector4Value#(Vector.Vector4, Entry)
  ;Diese Funktion gibt den Wert des angeben Vektors an der gegebenen Stelle zurück
  If Entry < 0 Or Entry > 3 Then Return 0

  Return Vector\Vector[Entry]
End Function

Function SetVector4Value#(Vector.Vector4, Entry, Value#)
  ;Diese Funktion legt den Wert des angeben Vektors an der gegebenen Stelle fest
  If Entry < 0 Or Entry > 3 Then Return 0

  Vector\Vector[Entry] = Value
End Function

Function AddVector4.Vector4(Vec1.Vector4, Vec2.Vector4)
  ;Diese Funktion addiert die beiden angeben Vektoren und gibt das Ergebnis zurück
   
  Local Vec.Vector4 = New Vector4

  For x = 0 To 3
   SetVector4Value(Vec,x,GetVector4Value(Vec1,x)+GetVector4Value(Vec2,x))
  Next
 
  Return Vec
End Function

Function AddVector4Skalar.Vector4(Vec1.Vector4, Value#)
  ;Diese Funktion addiert den angeben Vektor mit einem Skalar
   
  Local Vec.Vector4 = New Vector4

  For x = 0 To 3
   SetVector4Value(Vec,x,GetVector4Value(Vec1,x)+Value)
  Next
 
  Return Vec
End Function

Function MultiplyVector4.Vector4(Vec1.Vector4, Vec2.Vector4)
  ;Diese Funktion multipliziert die beiden angeben Vektoren miteinander und gibt das Ergebnis zurück
   
  Local Vec.Vector4 = New Vector4

  For x = 0 To 3
   SetVector4Value(Vec,x,GetVector4Value(Vec1,x)*GetVector4Value(Vec2,x))
  Next
 
  Return Vec
End Function

Function MultiplyVector4Skalar.Vector4(Vec1.Vector4, Value#)
  ;Diese Funktion multipliziert den angeben Vektor mit einem Skalar
   
  Local Vec.Vector4 = New Vector4

  For x = 0 To 3
   SetVector4Value(Vec,x,GetVector4Value(Vec1,x)*Value)
  Next
 
  Return Vec
End Function

Function NormalizeVector4(Vec1.Vector4)
  ;Diese Funktion normalisiert den angeben Vektor
  Local XSq#    = GetVector4Value(Vec1,0)*GetVector4Value(Vec1,0)
  Local YSq#    = GetVector4Value(Vec1,1)*GetVector4Value(Vec1,1)
  Local ZSq#    = GetVector4Value(Vec1,2)*GetVector4Value(Vec1,2)
  Local WSq#    = GetVector4Value(Vec1,3)*GetVector4Value(Vec1,3)
  Local LengthSq# = XSq + YSq + ZSq + WSq
  Local recip#
 
  If LengthSq > 1.0 Then
   recip = 1.0/Sqr(LengthSq)
   SetVector4Value(Vec1,0,GetVector4Value(Vec1,0)*recip)
   SetVector4Value(Vec1,1,GetVector4Value(Vec1,1)*recip)
   SetVector4Value(Vec1,2,GetVector4Value(Vec1,2)*recip)
   SetVector4Value(Vec1,3,GetVector4Value(Vec1,3)*recip)
  End If
End Function

Function Vector4Dot#(Vec1.Vector4, Vec2.Vector4)
  ;Diese Funktion berechnet das Punktprodukt der beiden Vektoren
  Local Value# = 0.0
 
  For x = 0 To 3
   Value = Value + GetVector4Value(Vec1,x)*GetVector4Value(Vec2,x)
  Next

  Return Value
End Function

Function Vector4Cross.Vector4(Vec1.Vector4, Vec2.Vector4)
  ;Diese Funktion berechnet das Kreuzprodukt der beiden Vektoren

  Local Vec.Vector4 = New Vector4

  Local NewX# = GetVector4Value(Vec1,1)*GetVector4Value(Vec2,2) - GetVector4Value(Vec1,2)*GetVector4Value(Vec2,1)
  Local NewY# = GetVector4Value(Vec1,2)*GetVector4Value(Vec2,0) - GetVector4Value(Vec1,0)*GetVector4Value(Vec2,2)
  Local NewZ# = GetVector4Value(Vec1,0)*GetVector4Value(Vec2,1) - GetVector4Value(Vec1,1)*GetVector4Value(Vec2,0)

  SetVector4Value(Vec,0,NewX)
  SetVector4Value(Vec,1,NewY)
  SetVector4Value(Vec,2,NewZ)

  Return Vec
End Function

;---------------Matrizen und Vektoren

Function MultiplyVector4Matrix4x4.Vector4(Vec1.Vector4, Mat.Matrix4x4)
  ;Diese Funktion mulitpliziert den angeben Vektor mit der angeben Matrix

  Local Vec.Vector4 = New Vector4
  Local Value# = 0.0
 
  For x = 0 To 3
   For y = 0 To 3
     Value = Value + GetVector4Value(Vec1,y)*GetMatrix4x4Value(Mat,y,x)
   Next
   
   SetVector4Value(Vec,x,Value)
   Value = 0.0
  Next

  Return Vec
End Function

Function SetMatrix4x4Row(Mat.Matrix4x4, Row, Vec.Vector4)
  ;Diese Funktion legt den Wert der Matrix an der angeben Spalte fest
  If Row < 0 Or Row > 3 Then Return
 
  For x = 0 To 3
    Mat\Mat[Row*4+x] = GetVector4Value(Vec,x)
  Next
End Function

;-------------------------------------------------------------------------------------------------------


;Ein kleines Beispiel

Graphics 800,600,0,2
SetBuffer BackBuffer()
SeedRnd MilliSecs()

Global Hermite.Matrix4x4 = New Matrix4x4

SetMatrix4x4Value(Hermite,0,0,2)
SetMatrix4x4Value(Hermite,0,1,-2)
SetMatrix4x4Value(Hermite,0,2,1)
SetMatrix4x4Value(Hermite,0,3,1)

SetMatrix4x4Value(Hermite,1,0,-3)
SetMatrix4x4Value(Hermite,1,1,3)
SetMatrix4x4Value(Hermite,1,2,-2)
SetMatrix4x4Value(Hermite,1,3,-1)

SetMatrix4x4Value(Hermite,2,2,1)
SetMatrix4x4Value(Hermite,3,0,1)
;Hermite = BuildTransponseMatrix4x4(Hermite)

debug$ = ""
For i = 0 To 3
  debug = ""
  For j = 0 To 3
    debug = debug + GetMatrix4x4Value(Hermite,i,j)+"   "
  Next
  DebugLog debug 
Next

Global SX = 0
Global SY = 0
Global EX = 800
Global EY = 600

Global Ang1 = 0
Global Pow1 = 40

Global Ang2 = 0
Global Pow2 = 40

Global SelectedID = 0

While Not KeyHit(1)
  Cls

  If MouseDown(1) Then
   If RectsOverlap(SX+Cos(Ang1)*Pow1,SY+Sin(Ang1)*Pow1,4,4,MouseX(),MouseY(),1,1) Then
     SelectedID = 1
   Else If RectsOverlap(EX+Cos(Ang2)*Pow2,EY+Sin(Ang2)*Pow2,4,4,MouseX(),MouseY(),1,1) Then
     SelectedID = 2
   End If
   If SelectedID = 0 Then
     SX = MouseX()
     SY = MouseY()
   Else If SelectedID = 1
     Pow1 = CalcDistance(MouseX(),MouseY(),SX,SY)
     Ang1 = ATan2(MouseY()-SY,MouseX()-SX)
   Else If SelectedID = 2
     Pow2 = CalcDistance(MouseX(),MouseY(),EX,EY)
     Ang2 = ATan2(MouseY()-EY,MouseX()-EX)
   End If
  Else If MouseDown(2) Then
    EX = MouseX()
    EY = MouseY()
  End If

  If MouseDown(1) = False Then SelectedID = 0
 
  Color 255,255,255
  DrawSpline(SX,SY,EX,EY,Cos(Ang1)*Pow1*5,Sin(Ang1)*Pow1*5,-Cos(Ang2)*Pow2*5,-Sin(Ang2)*Pow2*5)

 
  Color 255,0,0
  Rect SX-2,SY-2,4,4
  Rect EX-2,EY-2,4,4

  Color 0,255,0
  Rect SX+Cos(Ang1)*Pow1,SY+Sin(Ang1)*Pow1,4,4
  Rect EX+Cos(Ang2)*Pow2,EY+Sin(Ang2)*Pow2,4,4

Text 0,0,SelectedID
  Flip
Wend
End


Function DrawSpline(StartX,StartY,EndX,EndY,StartVelX#,StartVelY#,EndVelX#,EndVelY#)
  Start.Vector4 = New Vector4
  SetVector4Value(Start,0,StartX)
  SetVector4Value(Start,1,StartY)
  SetVector4Value(Start,2,0.0)
  SetVector4Value(Start,3,0.0)

  Ende.Vector4 = New Vector4
  SetVector4Value(Ende,0,EndX)
  SetVector4Value(Ende,1,EndY)
  SetVector4Value(Ende,2,0.0)
  SetVector4Value(Ende,3,0.0)

  StartVel.Vector4 = New Vector4
  SetVector4Value(StartVel,0,StartVelX)
  SetVector4Value(StartVel,1,StartVelY)
  SetVector4Value(StartVel,2,0.0)
  SetVector4Value(StartVel,3,0.0)

  EndVel.Vector4 = New Vector4
  SetVector4Value(EndVel,0,EndVelX)
  SetVector4Value(EndVel,1,EndVelY)
  SetVector4Value(EndVel,2,0.0)
  SetVector4Value(EndVel,3,0.0)

  Mat.Matrix4x4 = New Matrix4x4
  SetMatrix4x4Row(Mat,0,Start)
  SetMatrix4x4Row(Mat,1,Ende)
  SetMatrix4x4Row(Mat,2,StartVel)
  SetMatrix4x4Row(Mat,3,EndVel)

  oldx = StartX
  oldy = StartY

  For t# = 0.0 To 1.0 Step 0.02
    timeVec.Vector4 = New Vector4
    SetVector4Value(timeVec,0,t*t*t)
    SetVector4Value(timeVec,1,t*t)
    SetVector4Value(timeVec,2,t)
    SetVector4Value(timeVec,3,1.0)

    HermitVec.Vector4 = MultiplyVector4Matrix4x4(timeVec,Hermite) 

    Pos.Vector4 = MultiplyVector4Matrix4x4(HermitVec,Mat)
;DebugLog GetVector4Value(pos,0)+ "  "+GetVector4Value(pos,1)
    Line GetVector4Value(pos,0),GetVector4Value(pos,1),oldx,oldy
 ;   Rect GetVector4Value(pos,0),GetVector4Value(pos,1),5,5
    oldx = GetVector4Value(pos,0)
    oldy = GetVector4Value(pos,1)
 
   Delete HermitVec
   Delete timevec
   Delete pos
  Next

  Delete Start
  Delete Ende
  Delete StartVel
  Delete EndVel
  Delete Mat

End Function

Function CalcDistance(x1,y1,x2,y2)
  Return Sqr((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))
End Function

;Vektor und Matrizen Funktionssammlung
;by Steffen Nörtershäuser

;----------------------------------------Types----------------------------------------------------------

;Dieser Type verwaltet eine 4x4 Matrix
Type Matrix4x4
 Field Mat#[16]
End Type

Type Vector4
 Field Vector#[3]
End Type

;-------------------------------------------------------------------------------------------------------


;----------------------------------------Funktionen-----------------------------------------------------

;---------------Matrizen------------------------------

Function BuildIdentityMatrix4x4.Matrix4x4()
  ;Diese Funktion erstellt eine EinheitsMatrix und gibt diese zurück

  Local Mat.Matrix4x4 = New Matrix4x4

  For xy = 0 To 3
   SetMatrix4x4Value(Mat,xy,xy,1)
  Next

  Return Mat
End Function

Function BuildTransponseMatrix4x4.Matrix4x4(Mat1.Matrix4x4)
  ;Diese Funktion berechnet die Transponierte Matrix der angeben Matrix und gibt diese zurück

  Local Mat.Matrix4x4 = New Matrix4x4

  For x = 0 To 3
    For y = 0 To 3
     SetMatrix4x4Value(Mat,y,x,GetMatrix4x4Value(Mat1,x,y))
    Next
  Next

  Return Mat
End Function

Function CopyMatrix4x4.Matrix4x4(Mat1.Matrix4x4)
  ;Diese Funktion kopiert die angeben Matrix und gibt die Kopie zurück

  Local Mat.Matrix4x4 = New Matrix4x4

  For x = 0 To 3
    For y = 0 To 3
     SetMatrix4x4Value(Mat,x,y,GetMatrix4x4Value(Mat1,x,y))
    Next
  Next

  Return Mat
End Function

Function GetMatrix4x4Value#(Mat.Matrix4x4, Row, Column)
  ;Diese Funktion gibt den Wert der angeben Matrix an der gegebenen Stelle zurück
  If Row < 0 Or Row > 3 Then Return 0
  If Column < 0 Or Column> 3 Then Return 0

  Return Mat\Mat[Row*4+Column]
End Function

Function SetMatrix4x4Value(Mat.Matrix4x4, Row, Column, Value#)
  ;Diese Funktion gibt den Wert der angeben Matrix an der gegebenen Stelle zurück
  If Row < 0 Or Row > 3 Then Return
  If Column < 0 Or Column > 3 Then Return

  Mat\Mat[Row*4+Column] = Value
End Function

Function AddMatrix4x4.Matrix4x4(Mat1.Matrix4x4, Mat2.Matrix4x4)
  ;Diese Funktion addiert die beiden angeben Matrizen und gibt das Ergebnis zurück
   
  Local Mat.Matrix4x4 = New Matrix4x4

  For x = 0 To 3
    For y = 0 To 3
     SetMatrix4x4Value(Mat,x,y,GetMatrix4x4Value(Mat1,x,y)+GetMatrix4x4Value(Mat2,x,y))
    Next
  Next
 
  Return Mat
End Function

Function AddMatrix4x4Skalar.Matrix4x4(Mat1.Matrix4x4, Value#)
  ;Diese Funktion addiert die angebe Matrix mit dem angebenen Skalar und gibt das Ergebnis zurück
   
  Local Mat.Matrix4x4 = New Matrix4x4

  For x = 0 To 3
    For y = 0 To 3
     SetMatrix4x4Value(Mat,x,y,GetMatrix4x4Value(Mat1,x,y)+Value)
    Next
  Next
 
  Return Mat
End Function

Function MultiplyMatrix4x4.Matrix4x4(Mat1.Matrix4x4, Mat2.Matrix4x4)
  ;Diese Funktion multipliziert die beiden angeben Matrizen miteinander und gibt das Ergebnis zurück
   
  Local Mat.Matrix4x4 = New Matrix4x4
  Local Value# = 0.0
 
  For x = 0 To 3
    For y = 0 To 3

      For x2 = 0 To 3
       Value = Value + GetMatrix4x4Value(Mat1,x2,y)*GetMatrix4x4Value(Mat2,x,x2)
      Next
     
     SetMatrix4x4Value(Mat,x,y,Value)
     Value = 0.0
    Next
  Next

  Return Mat
End Function

Function MultiplyMatrix4x4Skalar.Matrix4x4(Mat1.Matrix4x4, Value#)
  ;Diese Funktion multipliziert die beiden angeben Matrizen und gibt das Ergebnis zurück
   
  Local Mat.Matrix4x4 = New Matrix4x4
 
  For x = 0 To 3
    For y = 0 To 3
     SetMatrix4x4Value(Mat,x,y,GetMatrix4x4Value(Mat1,x2,y)*Value)
    Next
  Next

  Return Mat
End Function

Function ExpMatrix4x4.Matrix4x4(Mat1.Matrix4x4, Expo)
  ;Diese Funktion potenziert die angebe Matrix und gibt das Ergebnis zurück

  Local Mat.Matrix4x4 = CopyMatrix4x4(Mat1)
  Local OldMat.Matrix4x4
  Local Value# = 0.0
 
  For CurExp = 2 To Expo
   OldMat = Mat
    Mat = MultiplyMatrix4x4(Mat,Mat)
    Delete OldMat
  Next

  Return Mat
End Function


;---------------Vektoren------------------------------

Function GetVector4Value#(Vector.Vector4, Entry)
  ;Diese Funktion gibt den Wert des angeben Vektors an der gegebenen Stelle zurück
  If Entry < 0 Or Entry > 3 Then Return 0

  Return Vector\Vector[Entry]
End Function

Function SetVector4Value#(Vector.Vector4, Entry, Value#)
  ;Diese Funktion legt den Wert des angeben Vektors an der gegebenen Stelle fest
  If Entry < 0 Or Entry > 3 Then Return 0

  Vector\Vector[Entry] = Value
End Function

Function AddVector4.Vector4(Vec.Vector4, Vec1.Vector4, Vec2.Vector4)
  ;Diese Funktion addiert die beiden angeben Vektoren und gibt das Ergebnis zurück
   
  For x = 0 To 3
   SetVector4Value(Vec,x,GetVector4Value(Vec1,x)+GetVector4Value(Vec2,x))
  Next
 
  Return Vec
End Function

Function AddVector4Skalar.Vector4(Vec.Vector4, Vec1.Vector4, Value#)
  ;Diese Funktion addiert den angeben Vektor mit einem Skalar

  For x = 0 To 3
   SetVector4Value(Vec,x,GetVector4Value(Vec1,x)+Value)
  Next
 
  Return Vec
End Function

Function SubstractVector4.Vector4(Vec.Vector4, Vec1.Vector4, Vec2.Vector4)
  ;Diese Funktion addiert die beiden angeben Vektoren und gibt das Ergebnis zurück
   
  For x = 0 To 3
   SetVector4Value(Vec,x,GetVector4Value(Vec1,x)-GetVector4Value(Vec2,x))
  Next
 
  Return Vec
End Function

Function MultiplyVector4.Vector4(Vec.Vector4, Vec1.Vector4, Vec2.Vector4)
  ;Diese Funktion multipliziert die beiden angeben Vektoren miteinander und gibt das Ergebnis zurück

  For x = 0 To 3
   SetVector4Value(Vec,x,GetVector4Value(Vec1,x)*GetVector4Value(Vec2,x))
  Next
 
  Return Vec
End Function

Function MultiplyVector4Skalar.Vector4(Vec.Vector4, Vec1.Vector4, Value#)
  ;Diese Funktion multipliziert den angeben Vektor mit einem Skalar
   
  For x = 0 To 3
   SetVector4Value(Vec,x,GetVector4Value(Vec1,x)*Value)
  Next
 
  Return Vec
End Function


Function DivideVector4Skalar.Vector4(Vec.Vector4, Vec1.Vector4, Value#)
  ;Diese Funktion multipliziert den angeben Vektor mit einem Skalar
   
  For x = 0 To 3
   SetVector4Value(Vec,x,GetVector4Value(Vec1,x)/Value)
  Next
 
  Return Vec
End Function


Function NormalizeVector4(Vec1.Vector4)
  ;Diese Funktion normalisiert den angeben Vektor
  Local XSq#    = GetVector4Value(Vec1,0)*GetVector4Value(Vec1,0)
  Local YSq#    = GetVector4Value(Vec1,1)*GetVector4Value(Vec1,1)
  Local ZSq#    = GetVector4Value(Vec1,2)*GetVector4Value(Vec1,2)
  Local WSq#    = GetVector4Value(Vec1,3)*GetVector4Value(Vec1,3)
  Local LengthSq# = XSq + YSq + ZSq + WSq
  Local recip#
 
  If LengthSq > 1.0 Then
   recip = 1.0/Sqr(LengthSq)
   SetVector4Value(Vec1,0,GetVector4Value(Vec1,0)*recip)
   SetVector4Value(Vec1,1,GetVector4Value(Vec1,1)*recip)
   SetVector4Value(Vec1,2,GetVector4Value(Vec1,2)*recip)
   SetVector4Value(Vec1,3,GetVector4Value(Vec1,3)*recip)
  End If
End Function

Function Vector4Dot#(Vec1.Vector4, Vec2.Vector4)
  ;Diese Funktion berechnet das Punktprodukt der beiden Vektoren
  Local Value# = 0.0
 
  For x = 0 To 3
   Value = Value + GetVector4Value(Vec1,x)*GetVector4Value(Vec2,x)
  Next

  Return Value
End Function

Function Vector4Cross.Vector4(Vec.Vector4, Vec1.Vector4, Vec2.Vector4)
  ;Diese Funktion berechnet das Kreuzprodukt der beiden Vektoren

  Local NewX# = GetVector4Value(Vec1,1)*GetVector4Value(Vec2,2) - GetVector4Value(Vec1,2)*GetVector4Value(Vec2,1)
  Local NewY# = GetVector4Value(Vec1,2)*GetVector4Value(Vec2,0) - GetVector4Value(Vec1,0)*GetVector4Value(Vec2,2)
  Local NewZ# = GetVector4Value(Vec1,0)*GetVector4Value(Vec2,1) - GetVector4Value(Vec1,1)*GetVector4Value(Vec2,0)

  SetVector4Value(Vec,0,NewX)
  SetVector4Value(Vec,1,NewY)
  SetVector4Value(Vec,2,NewZ)

  Return Vec
End Function

;---------------Matrizen und Vektoren

Function MultiplyVector4Matrix4x4.Vector4(Vec.Vector4, Vec1.Vector4, Mat.Matrix4x4)
  ;Diese Funktion mulitpliziert den angeben Vektor mit der angeben Matrix

  Local Value# = 0.0
 
  For x = 0 To 3
   For y = 0 To 3
     Value = Value + GetVector4Value(Vec1,y)*GetMatrix4x4Value(Mat,y,x)
   Next
   
   SetVector4Value(Vec,x,Value)
   Value = 0.0
  Next

  Return Vec
End Function

Function SetMatrix4x4Row(Mat.Matrix4x4, Row, Vec.Vector4)
  ;Diese Funktion legt den Wert der Matrix an der angeben Spalte fest
  If Row < 0 Or Row > 3 Then Return
 
  For x = 0 To 3
    Mat\Mat[Row*4+x] = GetVector4Value(Vec,x)
  Next
End Function


;-------------------------------------------------------------------------------------------------------


;Ein kleines Beispiel

Graphics3D 800,600,0,2
SetBuffer BackBuffer()
SeedRnd MilliSecs()

Global Hermite.Matrix4x4 = New Matrix4x4

SetMatrix4x4Value(Hermite,0,0,2)
SetMatrix4x4Value(Hermite,0,1,-2)
SetMatrix4x4Value(Hermite,0,2,1)
SetMatrix4x4Value(Hermite,0,3,1)

SetMatrix4x4Value(Hermite,1,0,-3)
SetMatrix4x4Value(Hermite,1,1,3)
SetMatrix4x4Value(Hermite,1,2,-2)
SetMatrix4x4Value(Hermite,1,3,-1)

SetMatrix4x4Value(Hermite,2,2,1)
SetMatrix4x4Value(Hermite,3,0,1)
;Hermite = BuildTransponseMatrix4x4(Hermite)

debug$ = ""
For i = 0 To 3
  debug = ""
  For j = 0 To 3
    debug = debug + GetMatrix4x4Value(Hermite,i,j)+"   "
  Next
  DebugLog debug
Next

Global SX = 0
Global SY = 0
Global EX = 800
Global EY = 600

Global Ang1 = 0
Global Pow1 = 40

Global Ang2 = 0
Global Pow2 = 40

Global SelectedID = 0

Global Cam = CreateCamera()

While Not KeyHit(1)
  Cls

  DrawSpline(0,0,0,10,50,500,100,200,50,0,0,0,Cam)
  HandelPlayerInput()
  Flip
Wend
End

Function HandelPlayerInput()
If KeyDown(17) Then MoveEntity Cam,0,0,1
If KeyDown(31) Then MoveEntity Cam,0,0,-1
If KeyDown(30) Then MoveEntity Cam,-1,0,0
If KeyDown(32) Then MoveEntity Cam,1,0,0

If MouseDown(2) Then
   Local MouseSpeedX = MouseXSpeed()
   Local MouseSpeedY = MouseYSpeed()
   
   If Abs(MouseSpeedX) > 5 Then MouseSpeedX = 5 * Sgn(MouseSpeedX)
   If Abs(MouseSpeedY) > 5 Then MouseSpeedY = 5 * Sgn(MouseSpeedY)
   
   TurnEntity Cam,MouseSpeedY,-MouseSpeedX,0
End If
RotateEntity Cam,EntityPitch(Cam),EntityYaw(Cam),0
End Function



Function DrawSpline(StartX#,StartY#,StartZ#,EndX#,EndY#,EndZ#,StartVelX#,StartVelY#,StartVelZ#,EndVelX#,EndVelY#,EndVelZ#,Cam)
  ;Diese Funktion zeichnet einen Spline

  Local Start.Vector4 = New Vector4
  SetVector4Value(Start,0,StartX)
  SetVector4Value(Start,1,StartY)
  SetVector4Value(Start,2,StartZ)
  SetVector4Value(Start,3,0.0)

  Local Ende.Vector4 = New Vector4
  SetVector4Value(Ende,0,EndX)
  SetVector4Value(Ende,1,EndY)
  SetVector4Value(Ende,2,EndZ)
  SetVector4Value(Ende,3,0.0)

  Local StartVel.Vector4 = New Vector4
  SetVector4Value(StartVel,0,StartVelX)
  SetVector4Value(StartVel,1,StartVelY)
  SetVector4Value(StartVel,2,StartVelZ)
  SetVector4Value(StartVel,3,0.0)

  Local EndVel.Vector4 = New Vector4
  SetVector4Value(EndVel,0,EndVelX)
  SetVector4Value(EndVel,1,EndVelY)
  SetVector4Value(EndVel,2,EndVelZ)
  SetVector4Value(EndVel,3,0.0)

  Local Mat.Matrix4x4 = New Matrix4x4
  SetMatrix4x4Row(Mat,0,Start)
  SetMatrix4x4Row(Mat,1,Ende)
  SetMatrix4x4Row(Mat,2,StartVel)
  SetMatrix4x4Row(Mat,3,EndVel)

  HermitVec.Vector4 = New Vector4
  Pos.Vector4 = New Vector4
  timeVec.Vector4 = New Vector4

  SetVector4Value(timeVec,0,0.0)
  SetVector4Value(timeVec,1,0.0)
  SetVector4Value(timeVec,2,0.0)
  SetVector4Value(timeVec,3,1.0)

  HermitVec.Vector4 = MultiplyVector4Matrix4x4(HermitVec,timeVec,Hermite) 

  Pos.Vector4 = MultiplyVector4Matrix4x4(Pos,HermitVec,Mat)
  CameraProject Cam,GetVector4Value(pos,0),GetVector4Value(pos,1),GetVector4Value(pos,2)

  Local oldx# = ProjectedX()
  Local oldy# = ProjectedY()

  For t# = 0.0 To 1.0 Step 0.02
    SetVector4Value(timeVec,0,t*t*t)
    SetVector4Value(timeVec,1,t*t)
    SetVector4Value(timeVec,2,t)
    SetVector4Value(timeVec,3,1.0)

    HermitVec.Vector4 = MultiplyVector4Matrix4x4(HermitVec,timeVec,Hermite) 

    Pos.Vector4 = MultiplyVector4Matrix4x4(Pos,HermitVec,Mat)

   CameraProject Cam,GetVector4Value(pos,0),GetVector4Value(pos,1),GetVector4Value(pos,2)
;DebugLog GetVector4Value(pos,0)+ "  "+GetVector4Value(pos,1)
;DebugLog ProjectedX()+"  "+ProjectedY()
    Line ProjectedX(),ProjectedY(),oldx,oldy
 ;   Rect GetVector4Value(pos,0),GetVector4Value(pos,1),5,5
    oldx = ProjectedX()
    oldy = ProjectedY()

  Next
 
  Delete HermitVec
  Delete timevec
  Delete pos

  Delete Start
  Delete Ende
  Delete StartVel
  Delete EndVel
  Delete Mat

End Function