.ghyp1 \ If we get here then A = 0 STA angleTangent \ Set angleTangent = 0 STA angleLo \ Set angle(Hi Lo) = 0 STA angleHi RTS \ Return from the subroutine .GetHypotenuseAngle \ We are going to be calculating the angle in a \ right-angled triangle with the following opposite and \ adjacent sides: \ \ (zDeltaAbsoluteHi zDeltaLo) \ \ (xDeltaAbsoluteHi xDeltaLo) \ \ We start by working out which is the longer of the two \ non-hypotenuse sides LDA zDeltaAbsoluteHi \ If zDeltaAbsoluteHi < xDeltaAbsoluteHi, jump to ghyp2 CMP xDeltaAbsoluteHi BCC ghyp2 BNE ghyp3 \ If zDeltaAbsoluteHi > xDeltaAbsoluteHi, jump to ghyp3 \ If we get here then the high bytes in zDeltaAbsoluteHi \ and xDeltaAbsoluteHi are the same, so now we compare \ the low bytes LDA zDeltaLo \ If zDeltaLo >= xDeltaLo, jump to ghyp3 CMP xDeltaLo BCS ghyp3 .ghyp2 \ If we get here then: \ \ (zDeltaAbsoluteHi zDeltaLo) < \ (xDeltaAbsoluteHi xDeltaLo) LDA zDeltaAbsoluteHi \ Set b(Hi Lo) = (zDeltaAbsoluteHi zDeltaLo) STA bHi \ LDA zDeltaLo \ So b(Hi Lo) is the shorter side STA bLo LDA xDeltaLo \ Set a(Hi Lo) = (xDeltaAbsoluteHi xDeltaLo) STA aLo \ LDA xDeltaAbsoluteHi \ So a(Hi Lo) is the longer side STA aHi JMP ghyp5 \ Jump to ghyp5 to keep going .ghyp3 \ If we get here then: \ \ (zDeltaAbsoluteHi zDeltaLo) >= \ (xDeltaAbsoluteHi xDeltaLo) LDA xDeltaAbsoluteHi \ Set b(Hi Lo) = (xDeltaAbsoluteHi xDeltaLo) STA bHi \ LDA xDeltaLo \ So b(Hi Lo) is the shorter side STA bLo LDA zDeltaLo \ Set a(Hi Lo) = (zDeltaAbsoluteHi zDeltaLo) STA aLo \ LDA zDeltaAbsoluteHi \ So a(Hi Lo) is the longer side STA aHi ORA zDeltaLo \ If both zDeltaAbsoluteHi and zDeltaLo are zero then BEQ ghyp1 \ a(Hi Lo) = 0, so jump to ghyp1 to return from the \ subroutine with the following: \ \ * angleTangent = 0 \ \ * angle(Hi Lo) = 0 LDA zDeltaAbsoluteHi \ Set A to zDeltaAbsoluteHi once again JMP ghyp9 \ Jump to ghyp9 to keep going .ghyp4 ASL zDeltaLo \ Shift (zDeltaAbsoluteHi zDeltaLo) left by one place to ROL zDeltaAbsoluteHi \ scale it up .ghyp5 \ If we jump here then: \ \ * (zDeltaAbsoluteHi zDeltaLo) < \ (xDeltaAbsoluteHi xDeltaLo) \ \ * a(Hi Lo) is the longer side \ \ * b(Hi Lo) is the shorter side \ \ * A = xDeltaAbsoluteHi \ \ The last one means that the length of the x-axis side \ is currently in (A xDeltaLo) \ We start by shifting the lengths of both sides to the \ left until bit 7 of A is set \ \ As we know that (A xDeltaLo) is the longest side, this \ scales both lengths up by the same amount until they \ are as large as they can be while still fitting into a \ 16-bit number ASL xDeltaLo \ Shift (A xDeltaLo) left by one place to scale it up ROL A BCC ghyp4 \ If we just shifted a zero out of bit 7 of (A xDeltaLo) \ then jump back to ghyp4 to scale the x-axis length and \ keep scaling until we shift a 1 out of bit 7, at which \ point we will have scaled (A xDeltaLo) as far as we \ can ROR A \ Shift the 1 back into bit 7 of (A xDeltaLo) to undo ROR xDeltaLo \ the last left-shift, so (A xDeltaLo) is now as large \ as it can be STA V \ Set V to the high byte of the scaled x-axis length LDA zDeltaLo \ Set T to the low byte of the scaled z-axis length STA T LDA xDeltaLo \ Set W to the low byte of the scaled x-axis length, AND #%11111100 \ with bits 0 and 1 cleared STA W \ \ Bits 0 and 1 of xDeltaLo will only be non-zero if the \ x-axis length has not been scaled up or has only been \ scaled up by one place, as scaling it up by two places \ will clear these bits anyway \ \ Is this a requirement for W so it can be passed to \ GetAngleFromCoords? LDA zDeltaAbsoluteHi \ Set A to the high byte of the scaled z-axis length \ So by this point we have: \ \ * (A T) = the scaled z-axis length \ \ * (V W) = the scaled x-axis length JSR GetAngleFromCoords \ Calculate the following angle: \ \ angle(Hi Lo) = arctan( (A T) / (V W) ) \ = arctan( z-axis / x-axis ) \ \ This works because the x-axis length is greater than \ the z-axis length, so the x-axis is the adjacent side \ and the z-axis is the opposite side LDA xDeltaHi \ If xDeltaHi and zDeltaHi have different sign bits in EOR zDeltaHi \ bit 7, then EOR'ing them will produce a 1, so jump to BMI ghyp6 \ ghyp6 to skip the following, as the sign of the angle \ is already correct LDA #0 \ Negate angle(Hi Lo) to give it the correct sign SEC SBC angleLo STA angleLo LDA #0 SBC angleHi STA angleHi .ghyp6 LDA #%01000000 \ If bit 7 of xDeltaHi is clear then set A = %01000000 BIT xDeltaHi \ otherwise set A = %11000000 BPL ghyp7 LDA #%11000000 .ghyp7 CLC \ Set angleHi = angleHi + A ADC angleHi \ STA angleHi \ So this sets bits 6 and 7 of the angle correctly so it \ is in the correct quadrant RTS \ Return from the subroutine .ghyp8 ASL xDeltaLo \ Shift (xDeltaAbsoluteHi xDeltaLo) left by one place to ROL xDeltaAbsoluteHi \ scale it up .ghyp9 \ If we jump here then: \ \ * (zDeltaAbsoluteHi zDeltaLo) >= \ (xDeltaAbsoluteHi xDeltaLo) \ \ * a(Hi Lo) is the longer side \ \ * b(Hi Lo) is the shorter side \ \ * A = zDeltaAbsoluteHi \ \ The last one means that the length of the z-axis side \ is currently in (A zDeltaLo) \ We start by shifting the lengths of both sides to the \ left until bit 7 of A is set \ \ As we know that (A zDeltaLo) is the longest side, this \ scales both lengths up by the same amount until they \ are as large as they can be while still fitting into a \ 16-bit number ASL zDeltaLo \ Shift (A zDeltaLo) left by one place to scale it up ROL A BCC ghyp8 \ If we just shifted a zero out of bit 7 of (A zDeltaLo) \ then jump back to ghyp8 to scale the z-axis length and \ keep scaling until we shift a 1 out of bit 7, at which \ point we will have scaled (A zDeltaLo) as far as we \ can ROR A \ Shift the 1 back into bit 7 of (A zDeltaLo) to undo ROR zDeltaLo \ the last left-shift, so (A zDeltaLo) is now as large \ as it can be STA V \ Set V to the high byte of the scaled z-axis length LDA xDeltaLo \ Set T to the low byte of the scaled x-axis length STA T LDA zDeltaLo \ Set W to the low byte of the scaled z-axis length, AND #&FC \ with bits 0 and 1 cleared STA W LDA xDeltaAbsoluteHi \ Set A to the high byte of the scaled x-axis length \ So by this point we have: \ \ * (A T) = the scaled x-axis length \ \ * (V W) = the scaled z-axis length JSR GetAngleFromCoords \ Calculate the following angle: \ \ angle(Hi Lo) = arctan( (A T) / (V W) ) \ = arctan( z-axis / x-axis ) \ \ This works because the z-axis length is greater than \ the x-axis length, so the z-axis is the adjacent side \ and the x-axis is the opposite side LDA xDeltaHi \ If xDeltaHi and zDeltaHi have the same sign bits in EOR zDeltaHi \ bit 7, then EOR'ing them will produce a 0, so jump to BPL ghyp10 \ ghyp10 to skip the following, as the sign of the angle \ is already correct \ \ This is because (z-axis / x-axis) will be positive as \ both sides of the division have the same sign, and as \ the arctangent of a positive value is also positive, \ we know we have the correct sign \ \ If each side of the division has a different sign then \ (z-axis / x-axis) will be negative, and so will the \ arctangent, so we need to negate the result LDA #0 \ Negate angle(Hi Lo) to give it the correct sign SEC SBC angleLo STA angleLo LDA #0 SBC angleHi STA angleHi .ghyp10 \ Finally, if the z-axis value is negative, then the \ hypotenuse will be pointing in the opposite direction \ to the z-axis (i.e. out of the screen rather than into \ it), so we need to add 180 degrees to the angle, which \ we can do by adding 128 LDA #0 \ If bit 7 of zDeltaHi is set then set A = 128 to add to BIT zDeltaHi \ the final result below, otherwise set A = 0 to the BPL ghyp11 \ result is unchanged LDA #128 .ghyp11 CLC \ Set angleHi = angleHi + A ADC angleHi STA angleHi \ So this sets bits 6 and 7 of the angle correctly so it \ is in the correct quadrant RTS \ Return from the subroutineName: GetHypotenuseAngle [Show more] Type: Subroutine Category: Maths (Geometry) Summary: Calculate the angle of the hypotenuse in a right-angle triangle given the two non-hypotenuse sides (i.e. two orthogonal axes)Context: See this subroutine in context in the source code References: This subroutine is called as follows: * GetObjectAngles calls GetHypotenuseAngle * GetObjPointAngles calls GetHypotenuseAngle * GetPitchAngleDelta calls GetHypotenuseAngle * GetTileViewAngles (Part 1 of 4) calls GetHypotenuseAngle
Calculate the tangent of the angle in a right-angle triangle, i.e. the angle of the hypotenuse, given the two non-hypotenuse sides (typically orthogonal coordinate axes). The arguments are as follows: * Absolute x-axis length in (xDeltaAbsoluteHi xDeltaLo) * Sign of the x-axis length in xDeltaHi * Absolute z-axis length in (zDeltaAbsoluteHi zDeltaLo) * Sign of the z-axis length in zDeltaHi
Arguments: xDeltaAbsoluteHi High byte of the absolute x-axis length xDelta(Hi Lo) The signed x-axis length zDeltaAbsoluteHi High byte of the absolute z-axis length zDelta(Hi Lo) The signed z-axis length
Returns: angleTangent The tangent of the angle of the hypotenuse angle(Hi Lo) The angle of the hypotenuse a(Hi Lo) The length of the longer side in the triangle b(Hi Lo) The length of the shorter side in the triangle
[X]
Subroutine GetAngleFromCoords (Part 1 of 3) (category: Maths (Geometry))
Given the coordinates along two axes, calculate the pitch or yaw angle to those coordinates
[X]
Variable angleTangent in workspace Zero page
The tangent of an angle in a right-angled triangle
[X]
Label ghyp1 is local to this routine
[X]
Label ghyp10 is local to this routine
[X]
Label ghyp11 is local to this routine
[X]
Label ghyp2 is local to this routine
[X]
Label ghyp3 is local to this routine
[X]
Label ghyp4 is local to this routine
[X]
Label ghyp5 is local to this routine
[X]
Label ghyp6 is local to this routine
[X]
Label ghyp7 is local to this routine
[X]
Label ghyp8 is local to this routine
[X]
Label ghyp9 is local to this routine
[X]
Variable xDeltaAbsoluteHi in workspace Zero page
The absolute difference between two x-coordinates (high byte), i.e. |xDeltaHi|
[X]
Variable zDeltaAbsoluteHi in workspace Zero page
The absolute difference between two z-coordinates (high byte), i.e. |zDeltaHi|