; File:   linf.s
; Author: Mikael Kalms <mikael@kalms.org>
; Date:   1 Oct 1999
; Title:  Linear algebra function library (FPU based)
;
; Description:
;   Contains functions for handling 3D vectors, 3x3 matrices,
;   3D transformations (kept as matrix + vector),
;   and 3D planes


; void	v3zerof(V3 *v);
; void	v3initf(V3 *v);
; void	v3copyf(V3 *v, V3 *v0);
; void	v3setf(V3 *v, float x, float y, float z);
; void	v3negf(V3 *v, V3 *v0);
; void	v3addf(V3 *v, V3 *v0, V3 *v1);
; void	v3subf(V3 *v, V3 *v0, V3 *v1);
; void	v3scalef(V3 *v, V3 *v0, float scale);
; void	v3project2df(V3 *v, V3 *v0);
; void	v3scalexyf(V3 *v, V3 *v0, float xscale, float yscale);
; void	v3scalexyzf(V3 *v, V3 *v0, float xscale, float yscale, float zscale);
; float	v3dotf(V3 *v0, V3 *v1);
; void	v3crossf(V3 *v, V3 *v0, V3 *v1);
; void	v3crossnormf(V3 *v, V3 *v0, V3 *v1);
; float	v3crosszf(V3 *v0, V3 *v1);
; float	v3crossnormzf(V3 *v0, V3 *v1);
; float	v3lenf(V3 *v0);
; (float v3lenfastf(V3 *v0);)
; void	v3normalizef(V3 *v, V3 *v0);
; void	v3setlenf(V3 *v, V3 *v0, float len);

; void	m3initf(M3 *m);
; void	m3copyf(M3 *m, M3 *m0);
; void	m3scalef(M3 *m, M3 *m0, float scale);
; void	m3v3mulf(M3 *m, M3 *m0, V3 *v0);
; void	m3mulf(M3 *m, M3 *m0, M3 *m1);
; void	m3setxyzscalef(M3 *m, float xscale, float yscale, float zscale);
; void	m3setxrotf(M3 *m, M3 *m0, float xangle);
; void	m3setyrotf(M3 *m, M3 *m0, float yangle);
; void	m3setzrotf(M3 *m, M3 *m0, float zangle);
; void	m3setzyxrotf(M3 *m, M3 *m0, float zangle, float yangle, float xangle);
; void	m3setbphrotf(M3 *m, M3 *m0, float bank, float pitch, float heading);
; void	m3sethpbrotf(M3 *m, M3 *m0, float heading, float pitch, float bank);
; void	m3transposef(M3 *m, M3 *m0);
; void	m3invertorthonormf(M3 *m, M3 *m0);
; void	m3invertf(M3 *m, M3 *m0);
; float	m3determinantf(M3 *m0);

; void	c3initf(C3 *c);
; void	c3setf(C3 *c, M3 *m0, V3 *v0);
; void	c3copyf(C3 *c, C3 *c0);
; void	c3mulf(C3 *c, C3 *c0, C3 *c1);
; void	c3v3mulf(C3 *c, C3 *c0, V3 *v0);
; void	c3v3rotf(C3 *c, C3 *c0, V3 *v0);
; void	c3setzxyrotmovef(C3 *c, float yxzangle, float xyzmove);
; void	c3setbphrotmovef(C3 *c, float hpbangle, float xyzmove);
; void	c3invertorthonormf(C3 *c, C3 *c0);
; void	c3invertf(C3 *c, C3 *c0);
; float	c3determinantf(C3 *c0);

; void	p3set_normreff(P3 *p, V3 *normal, V3 *reference);
; void	p3set_verticesf(P3 *p, V3 *v0, V3 *v1, V3 *v2);
; void	p3scalef(P3 *p, P3 *p0, float scale);
; void	p3normalizef(P3 *p, P3 *p0);
; void	p3setlenf(P3 *p, P3 *p0, float len);
; float	p3distf(P3 *p0, V3 *v0);
; void	p3flipf(P3 *p, P3 *p0);

	IFND	_LINF_I_
	include	3d/linf.i
	ENDC

	section	code,code

; a0	v3

v3zerof
v3initf
	clr.l	(a0)
	clr.l	4(a0)
	clr.l	8(a0)
	rts

; a0	v30
; a1	v3

v3copyf
	move.l	(a0),(a1)
	move.l	4(a0),4(a1)
	move.l	8(a0),8(a1)
	rts

; fp0	x
; fp1	y
; fp2	z
; a0	v3

v3setf
	fmove.s fp0,(a0)
	fmove.s fp1,4(a0)
	fmove.s fp2,8(a0)
	rts

; a0	v30
; a1	v3

v3negf
	fmove.s	(a0),fp0
	fmove.s	4(a0),fp1
	fmove.s	8(a0),fp2
	fneg	fp0
	fneg	fp1
	fneg	fp2
	fmove.s	fp0,(a1)
	fmove.s	fp1,4(a1)
	fmove.s	fp2,8(a1)
	rts

; a0	v30
; a1	v31
; a2	v3

v3v3addf
v3addf
	fmove.s	(a0),fp0
	fmove.s	4(a0),fp1
	fmove.s	8(a0),fp2
	fadd.s	(a1),fp0
	fadd.s	4(a1),fp1
	fadd.s	8(a1),fp2
	fmove.s	fp0,(a2)
	fmove.s	fp1,4(a2)
	fmove.s	fp2,8(a2)
	rts

; a0	v30
; a1	v31
; a2	v3

v3v3subf
v3subf
	fmove.s	(a0),fp0
	fmove.s	4(a0),fp1
	fmove.s	8(a0),fp2
	fsub.s	(a1),fp0
	fsub.s	4(a1),fp1
	fsub.s	8(a1),fp2
	fmove.s	fp0,(a2)
	fmove.s	fp1,4(a2)
	fmove.s	fp2,8(a2)
	rts

; fp0	scale
; a0	v30
; a1	v3

v3scalef
	fmove.s	(a0),fp1
	fmove.s	4(a0),fp2
	fmove.s	8(a0),fp3
	fmul	fp0,fp1
	fmul	fp0,fp2
	fmul	fp0,fp3
	fmove.s	fp1,(a1)
	fmove.s	fp2,4(a1)
	fmove.s	fp3,8(a1)
	rts

; a0	v30
; a1	v3

v3project2df
	tst.l	8(a0)
	ble.s	.nonplus
	fmove.s	(a0),fp0
	fmove.s	4(a0),fp1
	fmove.s	8(a0),fp2
	fdiv	fp2,fp0
	fdiv	fp2,fp1
	fmove.s	fp0,(a1)
	fmove.s	fp1,4(a1)
	fmove.s	fp2,8(a1)
	rts
.nonplus
	clr.l	(a1)
	clr.l	4(a1)
	clr.l	8(a1)
	rts

; fp0	xscale
; fp1	yscale
; a0	v30
; a1	v3

v3scalexyf
	fmul.s	(a0),fp0
	fmul.s	4(a0),fp1
	move.l	8(a0),8(a1)
	fmove.s	fp0,(a1)
	fmove.s	fp1,4(a1)
	rts

; fp0	xscale
; fp1	yscale
; fp2	zscale
; a0	v30
; a1	v3

v3scalexyzf
	fmul.s	(a0),fp0
	fmul.s	4(a0),fp1
	fmul.s	8(a0),fp2
	fmove.s	fp0,(a1)
	fmove.s	fp1,4(a1)
	fmove.s	fp2,8(a1)
	rts

; a0	v30
; a1	v31
; --
; fp0	dot

v3v3dotf
v3dotf
	fmove.s	(a0),fp0
	fmove.s	4(a0),fp1
	fmove.s	8(a0),fp2
	fmul.s	(a1),fp0
	fmul.s	4(a1),fp1
	fmul.s	8(a1),fp2
	fadd	fp2,fp0
	fadd	fp1,fp0
	rts

; a0	v30
; a1	v31
; a2	v3

v3v3crossf
v3crossf
	fmove.s	4(a0),fp0
	fmove.s	8(a0),fp1
	fmul.s	8(a1),fp0
	fmul.s	4(a1),fp1
	fsub	fp1,fp0
	fmove.s	8(a0),fp1
	fmove.s	(a0),fp2
	fmul.s	(a1),fp1
	fmul.s	8(a1),fp2
	fsub	fp2,fp1
	fmove.s	(a0),fp2
	fmove.s	4(a0),fp3
	fmul.s	4(a1),fp2
	fmul.s	(a1),fp3
	fsub	fp3,fp2
	fmove.l	fp0,(a2)
	fmove.l	fp1,4(a2)
	fmove.l	fp2,8(a2)
	rts

; a0	v30
; a1	v31
; a2	v3

v3v3crossnormf
v3crossnormf
	movem.l	a0-a1,-(sp)
	bsr.s	v3crossf
	move.l	a2,a0
	move.l	a2,a1
	bsr.s	v3normalizef
	movem.l	(sp)+,a0-a1
	rts

; a0	v30
; a1	v31
; --
; fp0	cross z value

v3v3crosszf
v3crosszf
	fmove.l	(a0),fp0
	fmove.l	4(a0),fp1
	fmul.l	4(a1),fp0
	fmul.l	(a1),fp1
	fsub	fp1,fp0
	rts

; a0	v30
; a1	v31
; a2	v3

v3v3crossnormzf
v3crossnormzf
	movem.l	a0-a1,-(sp)
	sub.w	#3*4*2,sp
	move.l	sp,a1
	bsr.s	v3normalizef
	move.l	3*4*2+4(sp),a0
	lea	3*4(sp),a1
	bsr.s	v3normalizef
	move.l	sp,a0
	lea	3*4(sp),a1
	bsr.s	v3crosszf
	add.w	#3*4*2,sp
	movem.l	(sp)+,a0-a1
	rts

; a0	v30
; --
; fp0	len

v3lenf
v3lenfastf
	move.l	a1,-(sp)
	move.l	a0,a1
	bsr	v3dotf
	fsqrt	fp0
	move.l	(sp)+,a1
	rts


; a0	v30
; a1	v3

v3normalizef
	fmove.b	#1,fp0
	bra.s	v3setlenf
	rts

; fp0	len
; a0	v30
; a1	v3

v3setlenf
	fmove.s	fp0,-(sp)
	bsr.s	v3lenf
	fmove.s	fp0,d0
	fmove.s	(sp)+,fp1
	tst.l	d0
	ble.s	.nonplus
	fdiv	fp0,fp1
	bsr	v3scalef
.nonplus
	rts

; a0	m3

m3initf
	move.l	a0,-(sp)
	move.l	#$3f800000,(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	move.l	#$3f800000,(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	move.l	#$3f800000,(a0)+
	move.l	(sp)+,a0
	rts

; a0	m30
; a1	m3

m3copyf
	movem.l	a0-a1,-(sp)
	REPT	9
	move.l	(a0)+,(a1)+
	ENDR
	movem.l	(sp)+,a0-a1
	rts

; fp0	scale
; a0	m30
; a1	m3

m3scalef
	movem.l	d2-d3/a0-a1,-(sp)
	moveq	#9-1,d3
.val	fmove.s	(a0)+,fp1
	fmul	fp0,fp1
	fmove.s	fp1,(a1)+
	dbf	d3,.val
	movem.l	(sp)+,d2-d3/a0-a1
	rts

; a0	m30
; a1	v30
; a2	v3

c3v3rotf
m3v3mulf
	move.l	a0,-(sp)
	fmove.s	(a0)+,fp0
	fmove.s	(a0)+,fp1
	fmove.s	(a0)+,fp2
	fmul.s	(a1),fp0
	fmul.s	4(a1),fp1
	fmul.s	8(a1),fp2
	fadd	fp2,fp0
	fadd	fp1,fp0
	fmove.s	(a0)+,fp1
	fmove.s	(a0)+,fp2
	fmove.s	(a0)+,fp3
	fmul.s	(a1),fp1
	fmul.s	4(a1),fp2
	fmul.s	8(a1),fp3
	fadd	fp3,fp1
	fadd	fp2,fp1
	fmove.s	(a0)+,fp2
	fmove.s	(a0)+,fp3
	fmul.s	(a1),fp2
	fmove.s	fp0,(a2)
	fmove.s	(a0)+,fp0
	fmul.s	4(a1),fp3
	fmul.s	8(a1),fp0
	fadd	fp3,fp0
	fadd	fp2,fp0
	fmove.s	fp1,4(a2)
	fmove.s	fp0,8(a2)
	move.l	(sp)+,a0
	rts

; a0	m30
; a1	m31
; a2	m3

m3m3mulf
m3mulf
	movem.l	d2-d4/a0-a1/a3,-(sp)
	sub.w	#9*4,sp
	move.l	sp,a3
	moveq	#3-1,d1
.y	moveq	#3-1,d0
.x
	fmove.s	(a0)+,fp0
	fmove.s	(a0)+,fp1
	fmove.s	(a0)+,fp2
	fmul.s	(a1)+,fp0
	fmul.s	(3-1)*4(a1),fp1
	fmul.s	(6-1)*4(a1),fp2
	fadd	fp2,fp0
	fadd	fp1,fp0
	fmove.s	fp0,(a3)+
	sub.w	#3*4,a0
	dbf	d0,.x
	add.w	#3*4,a0
	sub.w	#3*4,a1
	dbf	d1,.y
	movem.l	(sp)+,d0-d3
	movem.l	d0-d3,(a2)
	movem.l	(sp)+,d0-d4
	movem.l	d0-d4,4*4(a2)
	movem.l	(sp)+,d2-d4/a0-a1/a3
	rts

; fp0	xscale
; fp1	yscale
; fp2	zscale
; a0	m3

m3setxyzscalef
	move.l	a0,-(sp)
	fmove.s	fp0,(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	fmove.s	fp1,(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	fmove.s	fp2,(a0)+
	move.l	(sp)+,a0
	rts

; fp0	xangle
; a0	m3

m3setxrotf
	bsr	sincosf
	move.l	a0,-(sp)
	move.l	#$3f800000,(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	fmove.s	fp1,(a0)+
	fneg	fp0
	fmove.s	fp0,(a0)+
	fneg	fp0
	clr.l	(a0)+
	fmove.s	fp0,(a0)+
	fmove.s	fp1,(a0)+
	move.l	(sp)+,a0
	rts

; fp0	yangle
; a0	m3

m3setyrotf
	bsr	sincosf
	move.l	a0,-(sp)
	fmove.s	fp1,(a0)+
	clr.l	(a0)+
	fneg	fp0
	fmove.s	fp0,(a0)+
	fneg	fp0
	clr.l	(a0)+
	move.l	#$3f800000,(a0)+
	clr.l	(a0)+
	fmove.s	fp0,(a0)+
	clr.l	(a0)+
	fmove.s	fp1,(a0)+
	move.l	(sp)+,a0
	rts

; fp0	zangle
; a0	m3

m3setzrotf
	bsr	sincosf
	move.l	a0,-(sp)
	fmove.s	fp1,(a0)+
	fneg	fp0
	fmove.s	fp0,(a0)+
	fneg	fp0
	clr.l	(a0)+
	fmove.s	fp0,(a0)+
	fmove.s	fp1,(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	clr.l	(a0)+
	move.l	#$3f800000,(a0)+
	move.l	(sp)+,a0
	rts

; fp0	xangle
; fp1	yangle
; fp2	zangle
; a0	m3

m3setzyxrotf
	movem.l	a0-a2,-(sp)
	sub.w	#(9*2+3)*4,sp
	fmove.s	fp1,9*2*4(sp)
	fmove.s	fp2,(9*2+1)*4(sp)
	move.l	a0,(9*2+2)*4(sp)
	move.l	sp,a0
	bsr	m3setxrotf
	add.w	#9*4,a0
	fmove.s	9*2*4(sp),fp0
	bsr	m3setyrotf
	move.l	sp,a1
	move.l	a1,a2
	bsr	m3mulf
	fmove.s	(9*2+1)*4(sp),fp0
	bsr.s	m3setzrotf
	move.l	(9*2+2)*4(sp),a2
	bsr	m3mulf
	add.w	#(9*2+3)*4,sp
	movem.l	(sp)+,a0-a2
	rts

; fp0	hangle
; fp1	pangle
; fp2	bangle
; a0	m3

m3setzxyrotf
m3setbphrotf
	movem.l	a0-a2,-(sp)
	sub.w	#(9*2+3)*4,sp
	fmove.s	fp1,9*2*4(sp)
	fmove.s	fp2,(9*2+1)*4(sp)
	move.l	a0,(9*2+2)*4(sp)
	move.l	sp,a0
	bsr	m3setyrotf
	add.w	#9*4,a0
	fmove.s	9*2*4(sp),fp0
	bsr	m3setxrotf
	move.l	sp,a1
	move.l	a1,a2
	bsr	m3mulf
	fmove.s	(9*2+1)*4(sp),fp0
	bsr	m3setzrotf
	move.l	(9*2+2)*4(sp),a2
	bsr	m3mulf
	add.w	#(9*2+3)*4,sp
	movem.l	(sp)+,a0-a2
	rts

; fp0	hangle
; fp1	pangle
; fp2	bangle
; a0	m3

m3sethpbrotf
	movem.l	a0-a2,-(sp)
	sub.w	#(9*2+3)*4,sp
	fmove.s	fp1,9*2*4(sp)
	fmove.s	fp2,(9*2+1)*4(sp)
	move.l	a0,(9*2+2)*4(sp)
	move.l	sp,a0
	bsr	m3setyrotf
	add.w	#9*4,a0
	fmove.s	9*2*4(sp),fp0
	bsr	m3setxrotf
	move.l	a0,a1
	move.l	sp,a0
	move.l	a1,a2
	bsr	m3mulf
	fmove.s	(9*2+1)*4(sp),fp0
	bsr	m3setzrotf
	exg	a0,a1
	move.l	(9*2+2)*4(sp),a2
	bsr	m3mulf
	add.w	#(9*2+3)*4,sp
	movem.l	(sp)+,a0-a2
	rts

; a0	m30
; a1	m3

m3invertorthonormf
m3transposef
	move.l	4(a0),d0
	move.l	3*4(a0),d1
	move.l	d0,3*4(a1)
	move.l	d1,4(a1)
	move.l	2*4(a0),d0
	move.l	6*4(a0),d1
	move.l	d0,6*4(a1)
	move.l	d1,2*4(a1)
	move.l	5*4(a0),d0
	move.l	7*4(a0),d1
	move.l	d0,7*4(a1)
	move.l	d1,5*4(a1)
	move.l	(a0),(a1)
	move.l	4*4(a0),4*4(a1)
	move.l	8*4(a0),8*4(a1)
	rts

; a0	m30
; --
; fp0	determinant

m3determinantf
	movem.l	a0-a2,-(sp)
	add.w	#3*4,a0
	lea	3*4(a0),a1
	sub.w	#3*4,sp
	move.l	sp,a2
	bsr	m3cofactorrowf
	fmove.s	(a2)+,fp0
	fmove.s	(a2)+,fp1
	fmove.s	(a2)+,fp2
	fmul.s	-3*4(a0),fp0
	fmul.s	-2*4(a0),fp1
	fmul.s	-(a0),fp2
	fadd	fp2,fp0
	fsub	fp1,fp0
	add.w	#3*4,sp
	movem.l	(sp)+,a0-a2
	rts

; a0	m30
; a1	m3

m3invertf
	movem.l	a0-a2,-(sp)
	sub.w	#9*4,sp
	move.l	sp,a2
	move.l	a1,-(sp)
	bsr.s	m3determinantf
	fmove.s	fp0,-(sp)
	add.w	#3*4,a0
	lea	3*4(a0),a1
	bsr.s	m3cofactorrowf
	sub.w	#3*4,a0
	add.w	#3*4,a2
	bsr.s	m3cofactorrowf
	sub.w	#3*4,a1
	add.w	#3*4,a2
	bsr.s	m3cofactorrowf
	lea	-6*4(a2),a0
	eori.b	#$80,4(a0)
	eori.b	#$80,3*4(a0)
	eori.b	#$80,5*4(a0)
	eori.b	#$80,7*4(a0)
	move.l	a0,a1
	bsr	m3transposef
	fmove.b	#1,fp1
	fdiv	fp0,fp1
	fmove	fp1,fp0
	move.l	(sp)+,a1
	bsr	m3scalef
	add.w	#9*4,sp
	movem.l	(sp)+,a0-a2
	rts

; a0	row0
; a1	row1
; a2	row

m3cofactorrowf
	fmove.s	4(a0),fp0
	fmove.s	2*4(a0),fp1
	fmul.s	2*4(a1),fp0
	fmul.s	4(a1),fp1
	fsub	fp1,fp0
	fmove.s	(a0),fp1
	fmove.s	2*4(a0),fp2
	fmul.s	2*4(a1),fp1
	fmul.s	(a1),fp2
	fsub	fp2,fp1
	fmove.s	(a0),fp2
	fmove.s	4(a0),fp3
	fmul.s	4(a1),fp2
	fmul.s	(a1),fp3
	fsub	fp3,fp2
	fmove.l	fp0,(a2)
	fmove.l	fp1,4(a2)
	fmove.l	fp2,8(a2)
	rts

; a0	c3

c3initf
	bsr	m3initf
	add.w	#9*4,a0
	bsr	v3initf
	sub.w	#9*4,a0
	rts

; a0	m30
; a1	v30
; a2	c3

c3setf
	exg	a1,a2
	bsr	m3copyf
	exg	a0,a2
	add.w	#9*4,a1
	bsr	v3copyf
	sub.w	#9*4,a1
	exg	a0,a2
	exg	a1,a2
	rts

; a0	c30
; a1	c3

c3copyf
	movem.l	a0-a1,-(sp)
	REPT	12
	move.l	(a0)+,(a1)+
	ENDR
	movem.l	(sp)+,a0-a1
	rts

; a0	c30
; a1	c31
; a2	c3

c3c3mulf
c3mulf
	movem.l	a0-a2,-(sp)
	move.l	a2,-(sp)
	sub.w	#12*4,sp
	move.l	sp,a2
	bsr	m3mulf
	add.w	#9*4,a1
	add.w	#9*4,a2
	bsr	m3v3mulf
	add.w	#9*4,a0
	move.l	a2,a1
	bsr	v3addf
	move.l	sp,a0
	move.l	12*4(sp),a1
	bsr.s	c3copyf
	add.w	#(12+1)*4,sp
	movem.l	(sp)+,a0-a2
	rts

; a0	c30
; a1	v30
; a2	v3

c3v3mulf
	bsr	m3v3mulf
	add.w	#9*4,a0
	move.l	a1,-(sp)
	move.l	a2,a1
	bsr	v3addf
	move.l	(sp)+,a1
	sub.w	#9*4,a0
	rts

; a0	c3
; fp0	yangle
; fp1	xangle
; fp2	zangle
; fp3-fp5 move

c3setzxyrotmovef
c3setbphrotmovef
	bsr	m3setzxyrotf
	fmove.s	fp3,M3_SIZEOF+V3_X(a0)
	fmove.s	fp4,M3_SIZEOF+V3_Y(a0)
	fmove.s	fp5,M3_SIZEOF+V3_Z(a0)
	rts


; a0	c30
; a1	c3

c3invertf
	movem.l	a0-a2,-(sp)
	bsr	m3invertf
	add.w	#9*4,a0
	add.w	#9*4,a1
	bsr	v3negf
	lea	-9*4(a1),a0
	move.l	a1,a2
	bsr	m3v3mulf
	movem.l	(sp)+,a0-a2
	rts

; a0	c30
; a1	c3

c3invertorthonormf
	movem.l	a0-a2,-(sp)
	bsr	m3invertorthonormf
	add.w	#9*4,a0
	add.w	#9*4,a1
	bsr	v3negf
	lea	-9*4(a1),a0
	move.l	a1,a2
	bsr	m3v3mulf
	movem.l	(sp)+,a0-a2
	rts

; a0	c30
; --
; fp0	determinant

c3determinantf
	fmove.s	(a0),fp0
	fmove.s	2*4(a0),fp1
	fmul.s	8*4(a0),fp0
	fmul.s	7*4(a0),fp1
	fsub	fp1,fp0
	fmul.s	4*4(a0),fp0
	rts

; a0	normal
; a1	reference
; a2	p3

p3set_normreff
	move.l	(a0),(a2)
	move.l	4(a0),4(a2)
	move.l	8(a0),8(a2)
	bsr	v3dotf
	fneg	fp0
	fmove.s	fp0,P3_D(a2)
	rts

; a0	v30
; a1	v31
; a2	v32
; a3	p3

p3set_verticesf
	movem.l	a0-a2,-(sp)
	sub.w	#V3_SIZEOF*2,sp
	exg	a0,a1
	move.l	sp,a2
	bsr	v3subf
	move.l	V3_SIZEOF*2+2*4(sp),a0
	lea	V3_SIZEOF(sp),a2
	bsr	v3subf
	move.l	sp,a0
	move.l	a2,a1
	move.l	sp,a2
	bsr	v3crossnormf
	move.l	V3_SIZEOF*2(sp),a1
	move.l	a3,a2
	bsr.s	p3set_normreff
	add.w	#V3_SIZEOF*2,sp
	movem.l	(sp)+,a0-a2
	rts

; fp0	scale
; a0	p30
; a1	p3

p3scalef
	fmove.s	fp0,-(sp)
	bsr	v3scalef
	fmove.s	(sp)+,fp0
	fmul.s	P3_D(a0),fp0
	fneg	fp0
	fmove.s	fp0,P3_D(a1)
	rts


; a0	p30
; a1	p3

p3normalizef
	fmove.b	#1,fp0
;	bra.s	p3setlenf

; fp0	len
; a0	p30
; a1	p3

p3setlenf
	fmove.s	fp0,-(sp)
	bsr	v3lenf
	fmove.s	fp0,d0
	fmove.s	(sp)+,fp1
	tst.l	d0
	ble.s	.nonplus
	fdiv	fp0,fp1
	bsr.s	p3scalef
.nonplus
	rts

; a0	p30
; a1	v30
; --
; fp0	dist

p3distf
	bsr	v3dotf
	fadd.s	P3_D(a0),fp0
	rts

; a0	p30
; a1	p3

p3flip32
	bsr	v3negf
	fmove.s	P3_D(a0),fp0
	fneg	fp0
	fmove.s	fp0,P3_D(a1)
	rts

	section	data,data

c3_identity
m3_identity
	dc.l	$3f800000,0,0
	dc.l	0,$3f800000,0
	dc.l	0,0,$3f800000
	dc.l	0,0,0