| 1 | #include "3dmath.h" |
| 2 | |
| 3 | static void MultMatrix_neon( float m0[4][4], float m1[4][4], float dest[4][4]) |
| 4 | { |
| 5 | asm volatile ( |
| 6 | "vld1.32 {d0, d1}, [%1]! \n\t" //q0 = m1 |
| 7 | "vld1.32 {d2, d3}, [%1]! \n\t" //q1 = m1+4 |
| 8 | "vld1.32 {d4, d5}, [%1]! \n\t" //q2 = m1+8 |
| 9 | "vld1.32 {d6, d7}, [%1] \n\t" //q3 = m1+12 |
| 10 | "vld1.32 {d16, d17}, [%0]! \n\t" //q8 = m0 |
| 11 | "vld1.32 {d18, d19}, [%0]! \n\t" //q9 = m0+4 |
| 12 | "vld1.32 {d20, d21}, [%0]! \n\t" //q10 = m0+8 |
| 13 | "vld1.32 {d22, d23}, [%0] \n\t" //q11 = m0+12 |
| 14 | |
| 15 | "vmul.f32 q12, q8, d0[0] \n\t" //q12 = q8 * d0[0] |
| 16 | "vmul.f32 q13, q8, d2[0] \n\t" //q13 = q8 * d2[0] |
| 17 | "vmul.f32 q14, q8, d4[0] \n\t" //q14 = q8 * d4[0] |
| 18 | "vmul.f32 q15, q8, d6[0] \n\t" //q15 = q8 * d6[0] |
| 19 | "vmla.f32 q12, q9, d0[1] \n\t" //q12 = q9 * d0[1] |
| 20 | "vmla.f32 q13, q9, d2[1] \n\t" //q13 = q9 * d2[1] |
| 21 | "vmla.f32 q14, q9, d4[1] \n\t" //q14 = q9 * d4[1] |
| 22 | "vmla.f32 q15, q9, d6[1] \n\t" //q15 = q9 * d6[1] |
| 23 | "vmla.f32 q12, q10, d1[0] \n\t" //q12 = q10 * d0[0] |
| 24 | "vmla.f32 q13, q10, d3[0] \n\t" //q13 = q10 * d2[0] |
| 25 | "vmla.f32 q14, q10, d5[0] \n\t" //q14 = q10 * d4[0] |
| 26 | "vmla.f32 q15, q10, d7[0] \n\t" //q15 = q10 * d6[0] |
| 27 | "vmla.f32 q12, q11, d1[1] \n\t" //q12 = q11 * d0[1] |
| 28 | "vmla.f32 q13, q11, d3[1] \n\t" //q13 = q11 * d2[1] |
| 29 | "vmla.f32 q14, q11, d5[1] \n\t" //q14 = q11 * d4[1] |
| 30 | "vmla.f32 q15, q11, d7[1] \n\t" //q15 = q11 * d6[1] |
| 31 | |
| 32 | "vst1.32 {d24, d25}, [%2]! \n\t" //d = q12 |
| 33 | "vst1.32 {d26, d27}, [%2]! \n\t" //d+4 = q13 |
| 34 | "vst1.32 {d28, d29}, [%2]! \n\t" //d+8 = q14 |
| 35 | "vst1.32 {d30, d31}, [%2] \n\t" //d+12 = q15 |
| 36 | |
| 37 | :"+r"(m0), "+r"(m1), "+r"(dest): |
| 38 | : "d0", "d1", "d2", "d3", "d4", "d5", "d6", "d7", |
| 39 | "d16", "d17", "d18", "d19", "d20", "d21", "d22", "d23", |
| 40 | "d24", "d25", "d26", "d27", "d28", "d29", "d30", "d31", |
| 41 | "memory" |
| 42 | ); |
| 43 | } |
| 44 | |
| 45 | static void TransformVectorNormalize_neon(float vec[3], float mtx[4][4]) |
| 46 | { |
| 47 | asm volatile ( |
| 48 | "vld1.32 {d0}, [%1] \n\t" //Q0 = v |
| 49 | "flds s2, [%1, #8] \n\t" //Q0 = v |
| 50 | "vld1.32 {d18, d19}, [%0]! \n\t" //Q1 = m |
| 51 | "vld1.32 {d20, d21}, [%0]! \n\t" //Q2 = m+4 |
| 52 | "vld1.32 {d22, d23}, [%0] \n\t" //Q3 = m+8 |
| 53 | |
| 54 | "vmul.f32 q2, q9, d0[0] \n\t" //q2 = q9*Q0[0] |
| 55 | "vmla.f32 q2, q10, d0[1] \n\t" //Q5 += Q1*Q0[1] |
| 56 | "vmla.f32 q2, q11, d1[0] \n\t" //Q5 += Q2*Q0[2] |
| 57 | |
| 58 | "vmul.f32 d0, d4, d4 \n\t" //d0 = d0*d0 |
| 59 | "vpadd.f32 d0, d0, d0 \n\t" //d0 = d[0] + d[1] |
| 60 | "vmla.f32 d0, d5, d5 \n\t" //d0 = d0 + d1*d1 |
| 61 | |
| 62 | "vmov.f32 d1, d0 \n\t" //d1 = d0 |
| 63 | "vrsqrte.f32 d0, d0 \n\t" //d0 = ~ 1.0 / sqrt(d0) |
| 64 | "vmul.f32 d2, d0, d1 \n\t" //d2 = d0 * d1 |
| 65 | "vrsqrts.f32 d3, d2, d0 \n\t" //d3 = (3 - d0 * d2) / 2 |
| 66 | "vmul.f32 d0, d0, d3 \n\t" //d0 = d0 * d3 |
| 67 | "vmul.f32 d2, d0, d1 \n\t" //d2 = d0 * d1 |
| 68 | "vrsqrts.f32 d3, d2, d0 \n\t" //d3 = (3 - d0 * d3) / 2 |
| 69 | "vmul.f32 d0, d0, d3 \n\t" //d0 = d0 * d4 |
| 70 | |
| 71 | "vmul.f32 q2, q2, d0[0] \n\t" //d0= d2*d4 |
| 72 | |
| 73 | "vst1.32 {d4}, [%1] \n\t" //Q4 = m+12 |
| 74 | "fsts s10, [%1, #8] \n\t" //Q4 = m+12 |
| 75 | : "+r"(mtx): "r"(vec) |
| 76 | : "d0","d1","d2","d3","d18","d19","d20","d21","d22", "d23", "memory" |
| 77 | ); |
| 78 | } |
| 79 | |
| 80 | static void Normalize_neon(float v[3]) |
| 81 | { |
| 82 | asm volatile ( |
| 83 | "vld1.32 {d4}, [%0]! \n\t" //d4={x,y} |
| 84 | "flds s10, [%0] \n\t" //d5[0] = z |
| 85 | "sub %0, %0, #8 \n\t" //d5[0] = z |
| 86 | "vmul.f32 d0, d4, d4 \n\t" //d0= d4*d4 |
| 87 | "vpadd.f32 d0, d0, d0 \n\t" //d0 = d[0] + d[1] |
| 88 | "vmla.f32 d0, d5, d5 \n\t" //d0 = d0 + d5*d5 |
| 89 | |
| 90 | "vmov.f32 d1, d0 \n\t" //d1 = d0 |
| 91 | "vrsqrte.f32 d0, d0 \n\t" //d0 = ~ 1.0 / sqrt(d0) |
| 92 | "vmul.f32 d2, d0, d1 \n\t" //d2 = d0 * d1 |
| 93 | "vrsqrts.f32 d3, d2, d0 \n\t" //d3 = (3 - d0 * d2) / 2 |
| 94 | "vmul.f32 d0, d0, d3 \n\t" //d0 = d0 * d3 |
| 95 | "vmul.f32 d2, d0, d1 \n\t" //d2 = d0 * d1 |
| 96 | "vrsqrts.f32 d3, d2, d0 \n\t" //d3 = (3 - d0 * d3) / 2 |
| 97 | "vmul.f32 d0, d0, d3 \n\t" //d0 = d0 * d4 |
| 98 | |
| 99 | "vmul.f32 q2, q2, d0[0] \n\t" //d0= d2*d4 |
| 100 | "vst1.32 {d4}, [%0]! \n\t" //d2={x0,y0}, d3={z0, w0} |
| 101 | "fsts s10, [%0] \n\t" //d2={x0,y0}, d3={z0, w0} |
| 102 | |
| 103 | :"+r"(v) : |
| 104 | : "d0", "d1", "d2", "d3", "d4", "d5", "memory" |
| 105 | ); |
| 106 | } |
| 107 | |
| 108 | static float DotProduct_neon( float v0[3], float v1[3] ) |
| 109 | { |
| 110 | float dot; |
| 111 | asm volatile ( |
| 112 | "vld1.32 {d8}, [%1]! \n\t" //d8={x0,y0} |
| 113 | "vld1.32 {d10}, [%2]! \n\t" //d10={x1,y1} |
| 114 | "flds s18, [%1, #0] \n\t" //d9[0]={z0} |
| 115 | "flds s22, [%2, #0] \n\t" //d11[0]={z1} |
| 116 | "vmul.f32 d12, d8, d10 \n\t" //d0= d2*d4 |
| 117 | "vpadd.f32 d12, d12, d12 \n\t" //d0 = d[0] + d[1] |
| 118 | "vmla.f32 d12, d9, d11 \n\t" //d0 = d0 + d3*d5 |
| 119 | "fmrs %0, s24 \n\t" //r0 = s0 |
| 120 | : "=r"(dot), "+r"(v0), "+r"(v1): |
| 121 | : "d8", "d9", "d10", "d11", "d12" |
| 122 | |
| 123 | ); |
| 124 | return dot; |
| 125 | } |
| 126 | |
| 127 | void MathInitNeon() |
| 128 | { |
| 129 | MulMatrices = MultMatrix_neon; |
| 130 | //TransformVectorNormalize = TransformVectorNormalize_neon; |
| 131 | NormalizeVector = Normalize_neon; |
| 132 | DotProduct = DotProduct_neon; |
| 133 | } |