IrisQuaternion.ez80

#define	IRIS_VERSION_quat	$10
#if	IRIS_VERSION>=IRIS_VERSION_quat

; code start - Quaternions utility functions

#ifdef	IRIS_CORE_quatidentity
IQuaternionLoadIdentity:
	ex	de, hl
	ld	hl, IIdentityQuaternion
	ld	bc, IQUATERNION_SIZE
	ldir
	ex	de, hl
	ld	bc, -IQUATERNION_SIZE
	add	hl, bc
	ret
#endif
#ifdef	IRIS_EXT_quatloadrotation
IQuaternionLoadRotation:
	ret
#endif
#ifdef	IRIS_CORE_quatmlt
IQuaternionMlt:
; untested
	ld	hl, (ix+IQZ)
	ld	de, (iy+IQZ)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQX)
	ld	de, (iy+IQX)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQY)
	ld	de, (iy+IQY)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQW)
	ld	de, (iy+IQW)
	call	IQuatMlt
	pop	de
	or	a, a
	sbc	hl, de
	pop	de
	or	a, a
	sbc	hl, de
	pop	de
	or	a, a
	sbc	hl, de
; got IQW
	push	hl
	ld	hl, (ix+IQW)
	ld	de, (iy+IQX)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQX)
	ld	de, (iy+IQW)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQZ)
	ld	de, (iy+IQY)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQY)
	ld	de, (iy+IQZ)
	call	IQuatMlt
	pop	de
	or	a, a
	sbc	hl, de
	pop	de
	add	hl, de
	pop	de
	add	hl, de
; got IQX
	push	hl
	ld	hl, (ix+IQW)
	ld	de, (iy+IQY)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQX)
	ld	de, (iy+IQZ)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQY)
	ld	de, (iy+IQW)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQZ)
	ld	de, (iy+IQX)
	call	IQuatMlt
	pop	de
	add	hl, de
	pop	de
	or	a, a
	sbc	hl, de
	pop	de
	add	hl, de
; got IQY
	push	hl
	ld	hl, (ix+IQW)
	ld	de, (iy+IQZ)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQX)
	ld	de, (iy+IQY)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQY)
	ld	de, (iy+IQX)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQZ)
	ld	de, (iy+IQW)
	call	IQuatMlt
	pop	de
	or	a, a
	sbc	hl, de
	pop	de
	add	hl, de
	pop	de
	add	hl, de
; got IQZ
	ld	(ix+IQZ), hl
	pop	hl
	ld	(ix+IQY), hl
	pop	hl
	ld	(ix+IQX), hl
	pop	hl
	ld	(ix+IQW), hl
	ret
#endif
#ifdef	IRIS_EXT_quatmagnitude
IQuaternionMagnitude:
	ld	hl, (ix+IQW)
	call	IQuatSquare
	push	hl
	ld	hl, (ix+IQX)
	call	IQuatSquare
	push	hl
	ld	hl, (ix+IQY)
	call	IQuatSquare
	push	hl
	ld	hl, (ix+IQZ)
	call	IQuatSquare
	pop	de
	add	hl, de
	pop	de
	add	hl, de
	pop	de
	add	hl, de
	ret
#endif
#ifdef	IRIS_EXT_quatconjugate
IQuaternionConjugate:
	ld	de, (ix+3)
	or	a, a
	sbc	hl, hl
	sbc	hl, de
	ld	(ix+3), hl
	ld	de, (ix+6)
	or	a, a
	sbc	hl, hl
	sbc	hl, de
	ld	(ix+6), hl
	ld	de, (ix+9)
	or	a, a
	sbc	hl, hl
	sbc	hl, de
	ld	(ix+9), hl
	ret
#endif
#ifdef	IRIS_EXT_quatnormalize
IQuaternionNormalize:
	call	IQuatMagnitude
	call	ISquareRoot
; de = norm of the quaternion
; do quat.w/de

	ret
IQuatDivide:
; do HL*16384/DE
	ret
#endif
#ifdef	IRIS_CORE_quatgetmatrix
IQuaternionGetMatrix:
; iy quaternion, ix matrix
; (qw, qx, qy, qz)
; 1 - 2*qy² - 2*qz² 	2*qx*qy - 2*qz*qw 	2*qx*qz + 2*qy*qw
; 2*qx*qy + 2*qz*qw 	1 - 2*qx² - 2*qz² 	2*qy*qz - 2*qx*qw
; 2*qx*qz - 2*qy*qw 	2*qy*qz + 2*qx*qw 	1 - 2*qx² - 2*qy²
	ld	hl, (iy+IQZ)
	ld	de, (iy+IQW)
	call	IQuatMlt
	add	hl, hl
; 2*qz*qw
	push	hl
	ld	hl, (iy+IQX)
	ld	de, (iy+IQY)
	call	IQuatMlt
	add	hl, hl
; 2*qx*qy
	pop	de
	or	a, a
	sbc	hl, de
	ld	(ix+1), h
	add	hl, de
	add	hl, de
	ld	(ix+3), h
; next
	ld	hl, (iy+IQY)
	ld	de, (iy+IQW)
	call	IQuatMlt
	add	hl, hl
; 2*qw*qy
	push	hl
	ld	hl, (iy+IQX)
	ld	de, (iy+IQZ)
	call	IQuatMlt
	add	hl, hl
; 2*qx*qz
	pop	de
	or	a, a
	sbc	hl, de
	ld	(ix+6), h
	add	hl, de
	add	hl, de
	ld	(ix+2), h
; next
	ld	hl, (iy+IQX)
	ld	de, (iy+IQW)
	call	IQuatMlt
	add	hl, hl
; 2*qx*qw
	push	hl
	ld	hl, (iy+IQY)
	ld	de, (iy+IQZ)
	call	IQuatMlt
	add	hl, hl
; 2*qy*qz
	pop	de
	or	a, a
	sbc	hl, de
	ld	(ix+5), h
	add	hl, de
	add	hl, de
	ld	(ix+7), h
; 1-2*qy²-2qz²
	ld	hl, (iy+IQY)
	call	IQuatSquare
	push	hl
	push	hl
	ld	hl, (iy+IQZ)
	call	IQuatSquare
	ex	de, hl
	pop	bc
	ld	hl, $004000
	or	a, a
	sbc	hl, bc
	or	a, a
	sbc	hl, de
	ld	(ix+0), h
; 1-2*qz²-2qx²	
	push	de
	ld	hl, (iy+IQX)
	call	IQuatSquare
	ex	de, hl
	pop	bc
	ld	hl, $004000
	or	a, a
	sbc	hl, bc
	or	a, a
	sbc	hl, de
	ld	(ix+4), h
; 1-2qx²-2qy²
	pop	bc
	ld	hl, $004000
	or	a, a
	sbc	hl, bc
	or	a, a
	sbc	hl, de
	ld	(ix+8), h
	ret
#endif
#ifdef	IRIS_CORE_quatrotaxis
IQuaternionRotationAxis:
; iy adress of quaternion to write
; ix unit vector
; a angle
; qw = cos(angle/2)
; qx = axis.x*sin(angle/2)
; qy = axis.y*sin(angle/2)
; qz = axis.z*sin(angle/2)
	push	hl
	call	IQuatSin
; hl = sin
	ex	de, hl
; DE is never destroyed by the macro's
	ld	hl, (ix+0)
	call	IQuatMlt
	ld	(iy+IQX), hl
	ld	hl, (ix+3)
	call	IQuatMlt
	ld	(iy+IQY), hl
	ld	hl, (ix+6)
	call	IQuatMlt
	ld	(iy+IQZ), hl
	pop	hl
	call	IQuatCos
	ld	(iy+IQW), hl
	ret
#endif
#ifdef	IRIS_EXT_quatdot
IQuaternionDot:
	ld	hl, (ix+IQW)
	ld	de, (iy+IQW)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQX)
	ld	de, (iy+IQX)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQY)
	ld	de, (iy+IQY)
	call	IQuatMlt
	push	hl
	ld	hl, (ix+IQZ)
	ld	de, (iy+IQZ)
	call	IQuatMlt
	pop	de
	add	hl, de
	pop	de
	add	hl, de
	pop	de
	add	hl, de
	ret
#endif
#ifdef	IRIS_EXT_quatslerp
IQuaternionSlerp:
	ret
#endif

; always included functions to works with quaternions, called by the majority of the routines.

IQuatMlt:
; hl *de, de is not destroyed
	call	IFPMlt
; and now divide by 64
	add	hl, hl
	add	hl, hl
	ld	a, hlu
	ld	c, h
	add	hl, hl
	sbc	hl, hl
	ld	h, a
	ld	l, c	
	ret
IQuatSquare:
; bc is kept intact
	call	IFPSquare
; divide by 32
	add	hl, hl
	add	hl, hl
	add	hl, hl
	ld	a, hlu
	ld	e, h
	or	a, a
	sbc	hl, hl
	ld	h, a
	ld	l, e
	ret
IQuatCos:
	inc	h
IQuatSin:
	bit	0, h
	ld	a, l
	jr	z, IQuatSinSkp1
	neg
	jr	z, IQuatIndexZero
IQuatSinSkp1:
	bit	1, h
	jr	nz, IQuatSinNegate
	ld	hl, ISIN_TABLE
	ld	l, a
	ld	c, (hl)
	inc	h
	ld	b, (hl)
	or	a, a
	sbc	hl, hl
	ld	h, b
	ld	l, c
	ret
IQuatSinNegate:
	ld	hl, ISIN_TABLE
	inc.s	bc
	ld	l, a
	ld	c, (hl)
	inc	h
	ld	b, (hl)
	or	a, a
	sbc	hl, hl
	sbc	hl, bc
	ret
IQuatIndexZero:
	bit	1, h
	jr	nz, IQuatNegResultCst
	ld	hl, $004000
	ret
IQuatNegResultCst:
	ld	hl, $FFC000
	ret
#else
	.echo	"IrisQuaternion.ez80 version doesn't correspond to Iris3D.inc version"
#endif