'Classes/ActiveBasic/Math/Math.ab

Namespace ActiveBasic
Namespace Math
'----
'浮動小数点数補助
Function ldexp(x As Double, n As Long) As Double
	If x = 0 Then
		ldexp = 0
		Exit Function
	End If
	Dim pSrc = VarPtr(x) As *QWord
	Dim pDest = VarPtr(ldexp) As *QWord
	n += (pSrc[0] >> 52) As DWord And &h7FF
	pDest[0] = n << 52 Or (pSrc[0] And &h800FFFFFFFFFFFFF)
End Function

Function frexp(x As Double, ByRef n As Long) As Double
	If x = 0 Then
		n = 0
		frexp = 0
		Exit Function
	End If

	Dim pSrc = VarPtr(x) As *QWord
	Dim pDest = VarPtr(frexp) As *QWord
	n = ((pSrc[0] >> 52) As DWord And &h7FF) - 1022
	pDest[0] = (pSrc[0] And &h800FFFFFFFFFFFFF) Or &h3FE0000000000000
End Function

Function frexp(x As Single, ByRef n As Long) As Single
	If x = 0 Then
		n = 0
		frexp = 0
		Exit Function
	End If

	Dim pSrc As *DWord, pDest As *DWord
	pSrc = VarPtr(x) As *DWord
	pDest = VarPtr(frexp) As *DWord
	n = ((pSrc[0] >> 23) And &hFF) - 126
	pDest[0] = (pSrc[0] And &h807FFFFF) Or &h7E000000
End Function

'----
'冪乗
Function pow(x As Double, n As Long) As Double
	Dim abs_n As Long
	Dim r = 1 As Double

	abs_n=Abs(n) As Long
	While abs_n<>0
		If abs_n and 1 Then r *= x
		x = x * x
		abs_n >>= 1 ' abs_n \= 2
	Wend

	If n>=0 Then
		pow=r
	Else
		pow=1/r
	End If
End Function

Function pow(x As Double, y As Double) As Double
	Dim yi = y As Long
	If y = yi Then
		pow = pow(x, yi)
	ElseIf x>0 Then
		pow = Exp(y * Log(x))
		Exit Function
	ElseIf x<>0 or y<=0 Then
		pow = Detail.GetNaN()
	Else
		pow = 0
	End If
End Function

Function Sqrt(x As Double) As Double
	If x > 0 Then
		If IsInf(x) Then
			Sqrt = x
		Else
			Sqrt = x
			Dim i = (VarPtr(Sqrt) + 6) As *Word
			Dim jj = GetWord(i) As Long
			Dim j = jj >> 5 As Long
			Dim k = (jj And &h0000001f) As Long
			j = (j + 511) << 4 + k
			SetWord(i, j)
			Dim last As Double
			Do
				last = Sqrt
				Sqrt = (x / Sqrt + Sqrt) * 0.5
			Loop While Sqrt <> last
		End If
	ElseIf x < 0 Then
		Sqrt = Detail.GetNaN()
	Else
		'x = 0 Or NaN
		Sqrt = x
	End If
End Function

'xの符号だけをyのものにした値を返す。
'引数 x 元となる絶対値
'引数 y 元となる符号
Function CopySign(x As Double, y As Double) As Double
	SetQWord(VarPtr(CopySign), (GetQWord(VarPtr(x)) And &h7fffffffffffffff) Or (GetQWord(VarPtr(y)) And &h8000000000000000))
End Function

Function CopySign(x As Single, y As Single) As Single
	SetDWord(VarPtr(CopySign), (GetDWord(VarPtr(x)) And &h7fffffff) Or (GetDWord(VarPtr(y)) And &h80000000))
End Function

'----
'絶対値
Function Abs(n As Double) As Double
	If n < 0 Then
		Abs = -n
	Else
		Abs = n
	End If
End Function

Function Abs(n As Single) As Single
	If n < 0 Then
		Abs = -n
	Else
		Abs = n
	End If
End Function

Function Abs(n As Int64) As Int64
	If n < 0 Then
		Abs = -n
	Else
		Abs = n
	End If
End Function

Function Abs(n As Long) As Long
	If n < 0 Then
		Abs = -n
	Else
		Abs = n
	End If
End Function

Function Abs(n As Integer) As Integer
	If n < 0 Then
		Abs = -n
	Else
		Abs = n
	End If
End Function

Function Abs(n As SByte) As SByte
	If n < 0 Then
		Abs = -n
	Else
		Abs = n
	End If
End Function

'----
'指数・対数

Function Exp(x As Double) As Double
	If IsNaN(x) Then
		Return x
	Else If IsInf(x) Then
		If 0 > x Then
			Return 0
		Else
			Return x
		End If
	End If
	Dim k As Long
	If x >= 0 Then
		k = Fix(x / Detail._System_LOG2 + 0.5)
	Else
		k = Fix(x / Detail._System_LOG2 - 0.5)
	End If

	x -= k * Detail._System_LOG2

	Dim x2 = x * x
	Dim w = x2 / 22

	Dim i = 18
	While i >= 6
		w = x2 / (w + i)
		i -= 4
	Wend

	Return ldexp((2 + w + x) / (2 + w - x), k)
End Function

Function Log1p(x As Double) As Double
	If x < -1 Or IsNaN(x) Then
		Log1p = Detail.GetNaN()
	ElseIf x = 0 Then
		x = 0
	ElseIf IsInf(x) Then
		Log1p = x
	Else
		Log1p = Detail.Log1p(x)
	End If
End Function

Function Log(x As Double) As Double
	If x = 0 Then
		Log = Detail.GetInf(True)
	ElseIf x < 0 Or IsNaN(x) Then
		Log = Detail.GetNaN()
	ElseIf IsInf(x) Then
		Log = x
	Else
		Dim tmp = x * Detail._System_InverseSqrt2
		Dim p = VarPtr(tmp) As *QWord
		Dim m = GetQWord(p) And &h7FF0000000000000
		Dim k = ((m >> 52) As DWord) As Long - 1022
		SetQWord(p, m + &h0010000000000000)
		x /= tmp
		Log = Detail._System_LOG2 * k + Detail.Log1p(x - 1)
	End If
End Function

Function Log10(x As Double) As Double
	Return Log(x) * Detail._System_InverseLn10
End Function

'----
'三角関数
Function Sin(x As Double) As Double
	If IsNaN(x) Then
		Return x
	ElseIf IsInf(x) Then
		Return Detail.GetNaN()
		Exit Function
	End If

	Dim k As Long
	Dim t As Double

	t = Detail._Support_tan((x * 0.5) As Double, k)
	t = 2 * t / (1 + t * t)
	If (k And 1) = 0 Then 'k mod 2 = 0 Then
		Return t
	Else
		Return -t
	End If
End Function

Function Cos(x As Double) As Double
	If IsNaN(x) Then
		Return x
	ElseIf IsInf(x) Then
		Return Detail.GetNaN()
	End If

	Return Sin((Detail._System_HalfPI - Abs(x)) As Double)
End Function

Function Tan(x As Double) As Double
	If IsNaN(x) Then
		Tan = x
		Exit Function
	ElseIf IsInf(x) Then
		Tan = Detail.GetNaN()
		Exit Function
	End If

	Dim k As Long
	Dim t As Double
	t = Detail._Support_tan(x, k)
	If (k And 1) = 0 Then 'k mod 2 = 0 Then
		Return t
	ElseIf t <> 0 Then
		Return -1 / t
	Else
		Return CopySign(Detail.GetInf(False), -t)
	End If
End Function

'--
'三角関数の逆関数
Function Asin(x As Double) As Double
	If x < -1 Or x > 1 Then
		Asin = Detail.GetNaN()
	Else
		Asin = Atan(x / Sqrt(1 - x * x))
	End If
End Function

Function Acos(x As Double) As Double
	If x < -1 Or x > 1 Then
		Acos = Detail.GetNaN()
	Else
		Acos = Detail._System_HalfPI - Asin(x)
	End If
End Function

Function Atan(x As Double) As Double
	If IsNaN(x) Then
		Atan = x
		Exit Function
	ElseIf IsInf(x) Then
		Atan = CopySign(_System_PI, x)
		Exit Function
	End If
	Dim i As Long
	Dim sgn As Long
	Dim dbl = 0 As Double

	If x > 1 Then
		sgn = 1
		x = 1 / x
	ElseIf x < -1 Then
		sgn = -1
		x = 1 / x
	Else
		sgn = 0
	End If

	For i = Detail._System_Atan_N To 1 Step -1
		Dim t As Double
		t = i * x
		dbl = (t * t) / (2 * i + 1 + dbl)
	Next

	If sgn > 0 Then
		Atan = Detail._System_HalfPI - x / (1 + dbl)
	ElseIf sgn < 0 Then
		Atan = -Detail._System_HalfPI - x / (1 + dbl)
	Else
		Atan = x / (1 + dbl)
	End If
End Function

Function Atan2(y As Double, x As Double) As Double
	If x = 0 Then
		Atan2 = Sgn(y) * Detail._System_HalfPI
	Else
		Atan2 = Atn(y / x)
		If x < 0 Then
			Atan2 += CopySign(_System_PI, y)
		End If
	End If
End Function

'----
'双曲線関数
Function Sinh(x As Double) As Double
	If Abs(x) > Detail._System_EPS5 Then
		Dim t As Double
		t = Exp(x)
		Return (t - 1 / t) * 0.5
	Else
		Return x * (1 + x * x * 0.166666666666667) ' x * (1 + x * x / 6)
	End If
End Function

Function Tanh(x As Double) As Double
	If x > Detail._System_EPS5 Then
		Return 2 / (1 + Exp(-2 * x)) - 1
	ElseIf x < -Detail._System_EPS5 Then
		Return 1 - 2 / (Exp(2 * x) + 1)
	Else
		Return x * (1 - x * x * 0.333333333333333) 'x * (1 - x * x / 3)
	End If
End Function


'----
'浮動小数点数判定
Function IsNaN(ByVal x As Double) As Boolean
	Dim p = VarPtr(x) As *DWord
	IsNaN = False
	If (p[1] And &H7FF00000) = &H7FF00000 Then
		If (p[0] <> 0) Or ((p[1] And &HFFFFF) <> 0) Then
			IsNaN = True
		End If
	End If
End Function

Function IsInf(x As Double) As Boolean
	Dim p = VarPtr(x) As *DWord
	p[1] And= &h7fffffff
	Dim inf = Detail.GetInf(False)
	IsInf = (memcmp(p As *Byte, VarPtr(inf), SizeOf (Double)) = 0)
End Function

Function IsFinite(x As Double) As Boolean
	Dim p = VarPtr(x) As *DWord
	p[1] And= &H7FF00000
	IsFinite = ( p[1] And &H7FF00000 ) = &H7FF00000
End Function

'----
'その他
Function Hypot(x As Double, y As Double) As Double
	If x = 0 Then
		Hypot = Abs(y)
	Else If y = 0 Then
		Hypot = Abs(x)
	Else
		Dim ax = Abs(x)
		Dim ay = Abs(y)
		If ay > ax Then
			Dim t = x / y
			Hypot = ay * Sqr(1 + t * t)
		Else
			Dim t = y / x
			Hypot = ax * Sqr(1 + t * t)
		End If
	End If
End Function

Namespace Detail

Function SetSign(x As Double, isNegative As Long) As Double
#ifdef _WIN64
	SetQWord(AddressOf(CopySign), (GetQWord(VarPtr(x)) And &h7fffffffffffffff) Or (isNegative << 63))
#else
	SetDWord(AddressOf(CopySign), GetDWord(VarPtr(x)))
	SetDWord(AddressOf(CopySign) + SizeOf (DWord), GetQWord(VarPtr(x) + SizeOf (DWord)) And &h7fffffff Or (isNegative << 31))
#endif
End Function

#ifdef _WIN64

Function GetNaN() As Double
	SetQWord(VarPtr(GetNaN) As *QWord, &H7FF8000000000000)
End Function

Function GetInf(sign As Boolean) As Double
	Dim s = 0 As QWord
	If sign Then s = 1 << 63
	SetQWord(VarPtr(GetInf) As *QWord, &h7FF0000000000000 Or s)
End Function

#else

Function GetNaN() As Double
	Dim p = VarPtr(GetNaN) As *DWord
	p[0] = 0
	p[1] = &H7FF80000
End Function

Function GetInf(sign As Boolean) As Double
	Dim s = 0 As DWord
	If sign Then s = (1 As DWord) << 31
	Dim p = VarPtr(GetInf) As *DWord
	p[0] = 0
	p[1] = &h7FF00000 Or s
End Function

#endif

Function Log1p(x As Double) As Double
	Dim s = 0 As Double
	Dim i = _System_Log_N As Long
	While i >= 1
		Dim t = (i * x) As Double
		s = t * (2 * i + 1 + s) / (2 + t)
		i--
	Wend
	Return x / (1 + s)
End Function

Function _Support_tan(x As Double, ByRef k As Long) As Double
	If x >= 0 Then
		k = ( Fix(x * _System_InverseHalfPI) + 0.5 ) As Long
	Else
		k = ( Fix(x * _System_InverseHalfPI) - 0.5 ) As Long
	End If

	x = (x - (3217.0 / 2048.0) * k) + _System_D * k

	Dim x2 = x * x
	Dim t = 0 As Double

	Dim i As Long
	For i = _System_UrTan_N To 3 Step -2
		t = x2 / (i - t)
	Next

	_Support_tan = x / (1 - t)
End Function

Const _System_D = 4.4544551033807686783083602485579e-6 As Double
Const _System_UrTan_N = 19 As Long
Const _System_EPS5 = 0.001 As Double
Const _System_Atan_N = 20 As Long
Const _System_HalfPI = (_System_PI * 0.5)
Const _System_InverseHalfPI = (2 / _System_PI) '1 / (PI / 2)
Const _System_InverseLn10 = 0.43429448190325182765112891891661 '1 / (ln 10)
Const _System_InverseSqrt2 = 0.70710678118654752440084436210485 '1 / (√2)
Const _System_LOG2 = 0.6931471805599453094172321214581765680755
Const _System_Log_N = 7 As Long

End Namespace 'Detail
End Namespace 'Math
End Namespace 'ActiveBasic