| 1704 | |
| 1705 | template <class Float> |
| 1706 | void ComputeDiagonalBlockC(size_t ncam, float lambda1, float lambda2, |
| 1707 | const Float* jc, const int* cmap, const int* cmlist, |
| 1708 | Float* di, Float* bi, int vn, bool jc_transpose, |
| 1709 | bool use_jq, int mt) { |
| 1710 | const size_t bc = vn * 8; |
| 1711 | |
| 1712 | if (mt > 1 && ncam >= (size_t)mt) { |
| 1713 | MYTHREAD threads[THREAD_NUM_MAX]; |
| 1714 | const size_t thread_num = std::min(mt, THREAD_NUM_MAX); |
| 1715 | for (size_t i = 0; i < thread_num; ++i) { |
| 1716 | size_t first = ncam * i / thread_num; |
| 1717 | size_t last_ = ncam * (i + 1) / thread_num; |
| 1718 | size_t last = std::min(last_, ncam); |
| 1719 | RUN_THREAD(ComputeDiagonalBlockC, threads[i], (last - first), lambda1, |
| 1720 | lambda2, jc, cmap + first, cmlist, di + 8 * first, |
| 1721 | bi + bc * first, vn, jc_transpose, use_jq); |
| 1722 | } |
| 1723 | WAIT_THREAD(threads, thread_num); |
| 1724 | } else { |
| 1725 | Float bufv[64 + 8]; // size_t offset = ((size_t)bufv) & 0xf; |
| 1726 | // Float* pbuf = bufv + ((16 - offset) / sizeof(Float)); |
| 1727 | Float* pbuf = (Float*)ALIGN_PTR(bufv); |
| 1728 | |
| 1729 | ///////compute jc part |
| 1730 | for (size_t i = 0; i < ncam; ++i, ++cmap, bi += bc) { |
| 1731 | int idx1 = cmap[0], idx2 = cmap[1]; |
| 1732 | ////////////////////////////////////// |
| 1733 | if (idx1 == idx2) { |
| 1734 | SetVectorZero(bi, bi + bc); |
| 1735 | } else { |
| 1736 | SetVectorZero(pbuf, pbuf + 64); |
| 1737 | |
| 1738 | for (int j = idx1; j < idx2; ++j) { |
| 1739 | int idx = jc_transpose ? j : cmlist[j]; |
| 1740 | const Float* pj = jc + idx * 16; |
| 1741 | ///////////////////////////////// |
| 1742 | AddBlockJtJ(pj, pbuf, vn); |
| 1743 | AddBlockJtJ(pj + 8, pbuf, vn); |
| 1744 | } |
| 1745 | |
| 1746 | // change and copy the diagonal |
| 1747 | |
| 1748 | if (use_jq) { |
| 1749 | Float* pb = pbuf; |
| 1750 | for (int j = 0; j < 8; ++j, ++di, pb += 9) { |
| 1751 | Float temp; |
| 1752 | di[0] = temp = (di[0] + pb[0]); |
| 1753 | pb[0] = lambda2 * temp + lambda1; |
| 1754 | } |
| 1755 | } else { |
| 1756 | Float* pb = pbuf; |
| 1757 | for (int j = 0; j < 8; ++j, ++di, pb += 9) { |
| 1758 | *pb = lambda2 * ((*di) = (*pb)) + lambda1; |
| 1759 | } |
| 1760 | } |
| 1761 | |
| 1762 | // invert the matrix? |
| 1763 | if (vn == 8) |
no test coverage detected