Graphics3D 640,480,32,2
SetBuffer BackBuffer()
frametimer= CreateTimer(60)


Global dummy=CreatePivot()
Global cam=CreateCamera(dummy)
CameraClsColor cam,12,12,100
PositionEntity cam,0,0,-5
PointEntity cam,dummy
CameraZoom cam,4


Type lineType
Field mesh
Field alpha#
Field destroy
End Type
Global l.lineType


Global vec1#[3]
Global vec2#[3]
Global vec3#[3]
Global quat#[4]
Global quatInv#[4]
Global quatMult#[4]
Global vecQuat#[4]

winkel#=0
achsenneigung#=0
vektorneigung#=45


While Not KeyHit(1)


   
If KeyDown(30)  Then TurnEntity dummy,0,1,0
If KeyDown(32)  Then TurnEntity dummy,0,-1,0

   
If KeyDown(200)   Then achsenneigung = (achsenneigung+1) Mod 360
If KeyDown(208) Then achsenneigung = (achsenneigung-1) Mod 360
If KeyDown(203)   Then vektorneigung = (vektorneigung+1) Mod 360
If KeyDown(205) Then vektorneigung = (vektorneigung-1) Mod 360


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

winkel = (winkel+3) Mod 360   ; Rotationswinkel



vec1[1]=Cos#(vektorneigung)  ; Vektor der rotiert wird (ROT)
vec1[2]=Sin#(vektorneigung)
vec1[3]=0
TFormNormal vec1[1],vec1[2],vec1[3],0,0
vec1[1] = TFormedX#()
vec1[2] = TFormedY#()
vec1[3] = TFormedZ#()



vec2[1]=Cos#(achsenneigung)  ; Rotationsachse (GELB)
vec2[2]=Sin#(achsenneigung)
vec2[3]=0
TFormNormal vec2[1],vec2[2],vec2[3],0,0
vec2[1] = TFormedX#()
vec2[2] = TFormedY#()
vec2[3] = TFormedZ#()


QuaternionFromAxisAngle(vec2,winkel) ; Baue Quaternion aus Achse+Rotationswinkel

invertQuat(quat) ; invertiere Quaternion (notwendiger Zwischenschritt)

vecQuat[1] = vec1[1]   ; Baue Pseudo-Quaternion aus vec1, um ihn für die folgende Funktion passend zu machen
vecQuat[2] = vec1[2]
vecQuat[3] = vec1[3]
vecQuat[4] = 0.0

multiplyQuat(vecQuat,quatInv) ; multipliziere vec1 mit invertiertem Quaternion

multiplyQuat(quat,quatMult) ; multipliziere das Ergebnis von oben mit Quaternion

vec3[1] = quatMult[1]  ; vec3 ist der Ergebnisvektor!
vec3[2] = quatMult[2]
vec3[3] = quatMult[3]







l.lineType = New lineType                           ; Zeichne Vektoren
l\mesh = createLine(0,0,0,vec3[1],vec3[2],vec3[3],255,0,0)
l\destroy = 20
l\alpha = 1

l.lineType = New lineType
l\mesh = createLine(0,0,0,vec2[1],vec2[2],vec2[3],255,255,0)
l\destroy = 20
l\alpha = 1




For l.lineType = Each lineType

   l\alpha = l\alpha - 0.05
   EntityAlpha l\mesh,l\alpha
   l\destroy = l\destroy - 1
   If l\destroy = 0 Then
      FreeEntity l\mesh
      Delete l
   EndIf
   
Next


   CameraClsMode(cam,1,1)
   WireFrame(0)
   RenderWorld()


   CameraClsMode(cam,0,0)
   WireFrame(1)
   RenderWorld()
   
   
Text 10,10, "Vektor"   
Text 10,30, vec1[1]
Text 10,50, vec1[2]
Text 10,70, vec1[3]


Text 110,10, "Achse"
Text 110,30, vec2[1]
Text 110,50, vec2[2]
Text 110,70, vec2[3]      
   
   
Text 210,10, "Quaternion"   
Text 210,30, quat[1]
Text 210,50, quat[2]
Text 210,70, quat[3]
Text 210,90, quat[4]

      

Text 100,400, " Kamerasteuerung a,d"
Text 100,415, " Achse  kippen Pfeiltasten vertikal"
Text 100,430, " Vektor kippen Pfeiltasten horizontal"   



   WaitTimer frametimer
   
   Flip 0
   
   
Wend   



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

Function createLine(x1#,y1#,z1#, x2#,y2#,z2#, r,b,g, mesh=0)
   
   If mesh = 0 Then
      mesh=CreateMesh()
      EntityFX(mesh,16)
      surf=CreateSurface(mesh)   
      verts = 0   
   
      AddVertex surf,x1#,y1#,z1#,0,0
   Else
      surf = GetSurface(mesh,1)
      verts = CountVertices(surf)-1
   End If
   
   AddVertex surf,(x1#+x2#)/2,(y1#+y2#)/2,(z1#+z2#)/2,0,0

   AddVertex surf,x2#,y2#,z2#,1,0
   
   AddTriangle surf,verts,verts+2,verts+1
   
   EntityColor mesh,r,b,g

   
   Return mesh
End Function


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


Function QuaternionFromAxisAngle(axis#[3],angle#)

 
Local angle2# = angle/2.0

sinAngle# = Sin#(angle2)

quat[1] = axis[1]*sinAngle
quat[2] = axis[2]*sinAngle
quat[3] = axis[3]*sinAngle
quat[4] = Cos#(angle2)

End Function

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


Function invertQuat(quat#[4])

length# = 1.0/(quat[1]^2+quat[2]^2+quat[3]^2+quat[4]^2)

quatInv[1] = quat[1] * - length
quatInv[2] = quat[2] * - length
quatInv[3] = quat[3] * - length
quatInv[4] = quat[4] * length

End Function


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

Function multiplyQuat(quat1#[4],quat2#[4])

Local v1#[3]
Local v2#[3]
Local cross#[3]

v1[1] = quat1[1]
v1[2] = quat1[2]
v1[3] = quat1[3]
v2[1] = quat2[1]
v2[2] = quat2[2]
v2[3] = quat2[3]
Local angle# = (quat1[4]*quat2[4]) - (v1[1]*v2[1]+v1[2]*v2[2]+v1[3]*v2[3])

cross[1] = v1[2]*v2[3] - v1[3]*v2[2]
cross[2] = v1[3]*v2[1] - v1[1]*v2[3]
cross[3] = v1[1]*v2[2] - v1[2]*v2[1]

v1[1] = v1[1]*quat2[4]
v1[2] = v1[2]*quat2[4]
v1[3] = v1[3]*quat2[4]
v2[1] = v2[1]*quat1[4]
v2[2] = v2[2]*quat1[4]
v2[3] = v2[3]*quat1[4]

quatMult[1] = v1[1] + v2[1] + cross[1]
quatMult[2] = v1[2] + v2[2] + cross[2]
quatMult[3] = v1[3] + v2[3] + cross[3]
quatMult[4] = angle

End Function
;----------------------------------------------------------------------------------