*****************************************************************************
*
* CalcPull											(C) L. Vanhelsuwé
* --------
*
* Assembler module for GR gravity simulation.
* Calculate Gravitational Pull on mass Cloud[m].
*
* HISTORY
* -------
* 19-AUG-95: started this file out of frustration from Lattice's inability
*            to generate correct 68881 code itself.
*
*
*****************************************************************************


		XDEF	_asm_calc_pull				;what we export

		XREF	_CloudSize,_Cloud			;what we import

	; Mass STRUCTURE field offsets				;**!! track C's idea of structure!

mass		equ		0*8
x			equ		1*8
y			equ		2*8
z			equ		3*8
Vx			equ		4*8
Vy			equ		5*8
Vz			equ		6*8
mass_sizeof	equ		7*8


saved		equ		7*4						;number of regs saved

star		equ		saved+4
F_ptr		equ		saved+8
Fx_ptr		equ		saved+12
Fy_ptr		equ		saved+16
Fz_ptr		equ		saved+20

*--------------------------------------------------------------------------
* void	calc_pull (int star, num *F_ptr, num *Fx_ptr, num *Fy_ptr, num *Fz_ptr);
*
*--------------------------------------------------------------------------

	; Receives C arguments on stack as follows:
	;  4(SP) = mass index (0.. CloudSize -1)
	;  8(SP) = F_ptr
	; 12(SP) = Fx_ptr
	; 16(SP) = Fy_ptr
	; 20(SP) = Fz_ptr


_asm_calc_pull:	movem.l	d0/d7/a0-a4,-(SP)	;we're called from C, so save all !

				move.l	_CloudSize,d7		;scan entire Cloud
				subq.l	#1,d7

				move.l	_Cloud,a0			;-> array of Mass structures (the Cloud)

				move.l	star(SP),d0
				mulu	#mass_sizeof,d0
				lea		0(a0,d0.L),a1		;-> mass under consideration

				move.l	Fx_ptr(SP),a2		; = Fx ptr
				move.l	Fy_ptr(SP),a3		; = Fy ptr
				move.l	Fz_ptr(SP),a4		; = Fz ptr

				fmovecr	#$0F,fp0			; constant 0.0
				fmove.d	fp0,(a2)			; Fx = 0.0    (in double format)
				fmove.d	fp0,(a3)			; Fy = 0.0    (in double format)
				fmove.d	fp0,(a4)			; Fz = 0.0    (in double format)

				fmove.d	x(a1),fp0			; Cloud[star].x		! CACHED !
				fmove.d	y(a1),fp1			; Cloud[star].y
				fmove.d	z(a1),fp2			; Cloud[star].z

sum_vectors		cmp.l	a0,a1
				beq		skip_self

				fmove.d	x(a0),fp3
				fsub	fp0,fp3				; dx = Cloud[m].x - Cloud[star].x;

				fmove.d	y(a0),fp4
				fsub	fp1,fp4				; dy = Cloud[m].y - Cloud[star].y;

				fmove.d	z(a0),fp5
				fsub	fp2,fp5				; dz = Cloud[m].z - Cloud[star].z;

				fmove	fp3,fp7
				fmul	fp7,fp7				; dx^2

				fmove	fp4,fp6
				fmul	fp6,fp6				; dy^2
				fadd	fp6,fp7				; dx^2 + dy^2

				fmove	fp5,fp6
				fmul	fp6,fp6				; dz^2
				fadd	fp6,fp7				; dx^2 + dy^2 + dz^2 	(= R^2)

				ftst	fp7					; if (R^2 == 0) continue
				fbeq	skip_self

				fmove.d	mass(a0),fp6		; mass 1
				fmul.d	mass(a1),fp6		; mass 1 * mass 2
				fmul.l	#100,fp6			; GRAV * m1 * m2
				fdiv	fp7,fp6				; F = G*m1*m2/R^2

				fsqrt	fp7					; R = SQRT( R^2 )

;				ftst	fp7					; if (R == 0) continue
;				fbeq	skip_self

				fmul	fp6,fp3				; F*dx
				fdiv	fp7,fp3				; Fx = F*dx/R

				fmul	fp6,fp4				; F*dy
				fdiv	fp7,fp4				; Fy = F*dy/R

				fmul	fp6,fp5				; F*dz
				fdiv	fp7,fp5				; Fz = F*dz/R

				fmove.d	(a2),fp6			; accumulate into Fx,Fy,Fz
				fadd	fp3,fp6				; Fx += Fxcouple
				fmove.d	fp6,(a2)

				fmove.d	(a3),fp6
				fadd	fp4,fp6				; Fy += Fycouple
				fmove.d	fp6,(a3)

				fmove.d	(a4),fp6
				fadd	fp5,fp6				; Fz += Fzcouple
				fmove.d	fp6,(a4)

skip_self		add.w	#mass_sizeof,a0		; next Cloud element
				dbra	d7,sum_vectors

				fmove.d	(a2),fp0			;Fx
				fmul	fp0,fp0				;Fx^2

				fmove.d	(a3),fp1			;Fy
				fmul	fp1,fp1				;Fy^2

				fmove.d	(a4),fp2			;Fz
				fmul	fp2,fp2				;Fz^2

				fadd	fp2,fp1
				fadd	fp1,fp0				; Fx^2 + Fy^2 + Fz^2
				fsqrt	fp0					;SQRT ( Fx^2 + Fy^2 + Fz^2 )

				move.l	F_ptr(SP),a0
				fmove.d	fp0,(a0)			; return F

				movem.l	(SP)+,d0/d7/a0-a4

				rts

		END

*--------------------------------------------------------------------------
* Below is the original C source which this assembler routine replaces.
*--------------------------------------------------------------------------


	IFD	 THIS_IS_C_SOURCE

typedef struct Mass {
	num	mass;
	num	x,y,z;
	num	Vx,Vy,Vz;

} Mass;

//--------------------------------------------------------------------------
//--------------------------------------------------------------------------

void	calc_pull (int star, num *F_ptr, num *Fx_ptr, num *Fy_ptr, num *Fz_ptr) {

int m;
num	F,Fx,Fy,Fz;
num Fcouple;					// force magnitude between 2 masses
num	Fxcouple,Fycouple,Fzcouple;	// force vector
num	dx,dy,dz;
num	R;

		// reset force accumulators

	Fx = Fy = Fz = 0;

		// forall bodies (except body under consideration)

	for (m=0; m < CloudSize; m++) {

		if (m == star) continue;			// skip oneself...

			// calc distance between bodies

		dx = Cloud[m].x - Cloud[star].x;
		dy = Cloud[m].y - Cloud[star].y;
		dz = Cloud[m].z - Cloud[star].z;
	
		R = dx*dx + dy*dy + dz*dz;

			// If body collides with another, no force results
	
		if (R == 0) continue;
	
		Fcouple = GRAVITY_CONST * Cloud[star].mass * Cloud[m].mass / R;

		R = sqrt(R);
	
		if (R == 0) continue;
	
		Fxcouple = Fcouple * dx/R;
		Fycouple = Fcouple * dy/R;
		Fzcouple = Fcouple * dz/R;

		Fx += Fxcouple;
		Fy += Fycouple;
		Fz += Fzcouple;
	}

	F = sqrt( Fx*Fx + Fy*Fy + Fz*Fz );

	*F_ptr	= F;
	*Fx_ptr	= Fx;
	*Fy_ptr	= Fy;
	*Fz_ptr	= Fz;
}
//--------------------------------------------------------------------------
	ENDIF