void GSpringDamperBody::setOrientation(const SO3 &RL_, const SO3 &RR_)
{
T_left.SetRotation(RL_);
T_right.SetRotation(RR_);
inv_T_left = Inv(T_left);
inv_T_right = Inv(T_right);
}
void GSpringDamperBody::setPositionAndOrientation(const SE3 &TL_, const SE3 &TR_)
{
T_left = TL_;
T_right = TR_;
inv_T_left = Inv(T_left);
inv_T_right = Inv(T_right);
}
void GSpringDamperBody::setPosition(const Vec3 &pL_, const Vec3 &pR_)
{
T_left.SetPosition(pL_);
T_right.SetPosition(pR_);
inv_T_left = Inv(T_left);
inv_T_right = Inv(T_right);
}
void vpSystem::HDIteration2(void)
{
int i, j;
vpJoint *pCurrent, *pChild;
AInertia tmpI;
for ( i = m_pJoint.size() - 1; i >= 0; i-- )
{
pCurrent = m_pJoint[i];
pCurrent->m_sJ = pCurrent->m_sI;
pCurrent->m_sB = -dad(pCurrent->m_sV, pCurrent->m_sI * pCurrent->m_sV) - dAd(pCurrent->m_sRightBodyFrame, pCurrent->m_pRightBody->GetForce());
for ( j = 0; j < pCurrent->m_pChildJoints.size(); j++ )
{
pChild = pCurrent->m_pChildJoints[j];
if ( pChild->m_sHDType == VP::KINEMATIC )
{
pCurrent->m_sJ.AddTransform(pChild->m_sJ, Inv(pChild->m_sRelativeFrame));
} else
{
pChild->UpdateAInertia(tmpI);
pCurrent->m_sJ.AddTransform(tmpI, Inv(pChild->m_sRelativeFrame));
}
pCurrent->m_sB += InvdAd(pChild->m_sRelativeFrame, pChild->m_sC + pChild->GetLP());
}
pCurrent->m_sC = pCurrent->m_sB + pCurrent->m_sJ * pCurrent->m_sW;
if ( pCurrent->m_sHDType == VP::KINEMATIC ) pCurrent->UpdateLP();
else pCurrent->UpdateLOTP();
}
if ( m_pRoot->m_bIsGround ) return;
if ( m_pRoot->m_pJoint.size() ) m_sRootInertia = m_pRoot->m_sI;
m_sRootBias = -dad(m_pRoot->m_sV, m_pRoot->m_sI * m_pRoot->m_sV);
if ( m_pRoot->m_sHDType == VP::DYNAMIC ) m_sRootBias -= m_pRoot->GetForce();
else m_sRootBias -= m_pRoot->GetGravityForce();
for ( i = 0; i < m_pRoot->m_pJoint.size(); i++ )
{
pChild = m_pRoot->m_pJoint[i];
if ( pChild->m_sHDType == VP::KINEMATIC )
{
m_sRootInertia.AddTransform(pChild->m_sJ, Inv(pChild->m_sRelativeFrame));
} else
{
pChild->UpdateAInertia(tmpI);
m_sRootInertia.AddTransform(tmpI, Inv(pChild->m_sRelativeFrame));
}
m_sRootBias += InvdAd(pChild->m_sRelativeFrame, pChild->m_sC + pChild->GetLP());
}
}
inline void push_down(int x)
{
int l=t[x].ls,r=t[x].rs;
Inv(l);
Inv(r);
t[x].tag=0;
push_up(x);
}
void MultiCollide(const Triangle &tri0, const Triangle &tri1, const Triangle &tri2, const Triangle &tri3,
ShadowContext &ctx, int sidx, int firstA, int lastA) {
//TODO: ulozyc odpowiednio (wektorowo) i liczyc test RayInterval - 4 trojkaty
Vec3q tnrm[4], tvec0[4], tvec1[4];
floatq tmul[4], zero(0.0f), one(1.0f);
tri0.Prepare(ctx, tnrm[0], tvec0[0], tvec1[0], tmul[0]);
tri1.Prepare(ctx, tnrm[1], tvec0[1], tvec1[1], tmul[1]);
tri2.Prepare(ctx, tnrm[2], tvec0[2], tvec1[2], tmul[2]);
tri3.Prepare(ctx, tnrm[3], tvec0[3], tvec1[3], tmul[3]);
for(int q = firstA; q <= lastA; q++) {
Vec3q dir = ctx.rayDir[q];
floatq distance = ctx.distance[q];
floatq idet[4] = { Inv(dir | tnrm[0]), Inv(dir | tnrm[1]), Inv(dir | tnrm[2]), Inv(dir | tnrm[3]) };
floatq dist[4] = { idet[0] * tmul[0], idet[1] * tmul[1], idet[2] * tmul[2], idet[3] * tmul[3] };
floatq v[4] = {
(dir | tvec0[0]) * idet[0],
(dir | tvec0[1]) * idet[1],
(dir | tvec0[2]) * idet[2],
(dir | tvec0[3]) * idet[3],
};
floatq u[4] = {
(dir | tvec1[0]) * idet[0],
(dir | tvec1[1]) * idet[1],
(dir | tvec1[2]) * idet[2],
(dir | tvec1[3]) * idet[3],
};
f32x4b test[4] = {
Min(u[0], v[0]) >= zero && u[0] + v[0] <= one,
Min(u[1], v[1]) >= zero && u[1] + v[1] <= one,
Min(u[2], v[2]) >= zero && u[2] + v[2] <= one,
Min(u[3], v[3]) >= zero && u[3] + v[3] <= one,
};
test[0] = test[0] /*&& idet[0] > zero*/ && dist[0] >= zero;
test[1] = test[1] /*&& idet[1] > zero*/ && dist[1] >= zero;
test[2] = test[2] /*&& idet[2] > zero*/ && dist[2] >= zero;
test[3] = test[3] /*&& idet[3] > zero*/ && dist[3] >= zero;
f32x4 minDist = distance;
minDist = Condition(test[0] && dist[0] < minDist, dist[0], minDist);
minDist = Condition(test[1] && dist[1] < minDist, dist[1], minDist);
minDist = Condition(test[2] && dist[2] < minDist, dist[2], minDist);
minDist = Condition(test[3] && dist[3] < minDist, dist[3], minDist);
test[0] = test[0] && dist[0] <= minDist;
test[1] = test[1] && dist[1] <= minDist;
test[2] = test[2] && dist[2] <= minDist;
test[3] = test[3] && dist[3] <= minDist;
ctx.distance[q] = minDist;
}
}
// 現在の局面の評価値の内訳を表示する。
void print_eval_stat(Position& pos)
{
cout << "--- EVAL STAT\n";
Square sq_bk0 = pos.king_square(BLACK);
Square sq_wk1 = Inv(pos.king_square(WHITE));
auto list_fb = pos.eval_list()->piece_list_fb();
auto list_fw = pos.eval_list()->piece_list_fw();
int i, j;
BonaPiece k0, k1;
// 38枚の駒を表示
for (i = 0; i < PIECE_NO_KING; ++i)
cout << int(list_fb[i]) << " = " << list_fb[i] << " , " << int(list_fw[i]) << " = " << list_fw[i] << endl;
int32_t sumBKPP, sumWKPP, sumKKP;
cout << "KKC : " << sq_bk0 << " " << Inv(sq_wk1) << " = " << kkp[sq_bk0][sq_wk1][fe_end] << "\n";
sumBKPP = sumWKPP = 0;
sumKKP = kkp[sq_bk0][sq_wk1][fe_end];
for (i = 0; i < PIECE_NO_KING; i++)
{
k0 = list_fb[i];
k1 = list_fw[i];
cout << "KKP : " << sq_bk0 << " " << Inv(sq_wk1) << " " << k0 << " = " << kkp[sq_bk0][sq_wk1][k0] << "\n";
sumKKP += kkp[sq_bk0][sq_wk1][k0];
for (j = 0; j <= i; j++)
{
cout << "BKPP : " << sq_bk0 << " " << k0 << " " << list_fb[j] << " = " << kpp[sq_bk0][k0][list_fb[j]] << "\n";
cout << "WKPP : " << sq_wk1 << " " << k1 << " " << list_fw[j] << " = " << kpp[sq_wk1][k1][list_fw[j]] << "\n";
sumBKPP += kpp[sq_bk0][k0][list_fb[j]];
sumWKPP += kpp[sq_wk1][k1][list_fw[j]];
// cout << "sumWKPP = " << sumWKPP << " sumBKPP " << sumBKPP << " sumWKPP " << sumWKPP << endl;
// i==jにおいて0以外やったらあかんで!!
ASSERT(!(i == j && kpp[sq_bk0][k0][list_fb[j]] != 0));
}
}
cout << "Material = " << pos.state()->materialValue << endl;
cout << "sumKKP = " << sumKKP << " sumBKPP " << sumBKPP << " sumWKPP " << sumWKPP << endl;
cout << "---\n";
}
// Articulated Inertia Forward Dynamics Aglorithm for tree structures
// the second step : inboard iteration
void vpSystem::FDIteration2(void)
{
int i, j;
vpJoint *pCurrent, *pChild;
dse3 tmp_b;
AInertia tmpI;
for ( i = m_pJoint.size() - 1; i >= 0; i-- )
{
pCurrent = m_pJoint[i];
pCurrent->m_sJ = pCurrent->m_sI;
pCurrent->m_sB.dad(pCurrent->m_sV, pCurrent->m_sI * pCurrent->m_sV);
pCurrent->m_sB *= -SCALAR_1;
for ( j = 0; j < pCurrent->m_pChildJoints.size(); j++ )
{
pChild = pCurrent->m_pChildJoints[j];
pChild->UpdateAInertia(tmpI);
pCurrent->m_sJ.AddTransform(tmpI, Inv(pChild->m_sRelativeFrame));
pCurrent->m_sB += InvdAd(pChild->m_sRelativeFrame, pChild->m_sC + pChild->GetLP());
}
tmp_b.dAd(pCurrent->m_sRightBodyFrame, pCurrent->m_pRightBody->GetForce());
pCurrent->m_sB -= tmp_b;
pCurrent->m_sC = pCurrent->m_sJ * pCurrent->m_sW;
pCurrent->m_sC += pCurrent->m_sB;
pCurrent->UpdateLOTP();
}
if ( m_pRoot->m_bIsGround ) return;
if ( m_pRoot->m_pJoint.size() ) m_sRootInertia = m_pRoot->m_sI;
m_sRootBias.dad(-m_pRoot->m_sV, m_pRoot->m_sI * m_pRoot->m_sV);
for ( i = 0; i < m_pRoot->m_pJoint.size(); i++ )
{
pChild = m_pRoot->m_pJoint[i];
pChild->UpdateAInertia(tmpI);
m_sRootInertia.AddTransform(tmpI, Inv(pChild->m_sRelativeFrame));
m_sRootBias += InvdAd(pChild->m_sRelativeFrame, pChild->m_sC + pChild->GetLP());
}
m_sRootBias -= m_pRoot->GetForce();
}
PieceNumber Position::make_list_drop(Piece piece, Square to)
{
// 持ち駒の中で一番駒番号の多い駒を打ちます。
const int count = handcount[piece];
const int handIndex0 = NanohaTbl::HandIndex0[piece] + count;
const PieceNumber kn = listkn[handIndex0]; // maxの駒番号
assert(handIndex0 < fe_hand_end);
// knをセーブ
st->oldlist[0] = list0[kn];
st->oldlist[1] = list1[kn];
listkn[handIndex0] = PIECENUMBER_NONE; // 駒番号の一番大きい持ち駒を消去
handcount[piece]--; // 打つので１枚減らす
// 打った駒の情報
const int sq = conv_z2sq(to);
list0[kn] = NanohaTbl::KppIndex0[piece] + sq;
list1[kn] = NanohaTbl::KppIndex1[piece] + Inv(sq);
#if defined(EVAL_DIFF)
st->newlist[0] = list0[kn];
st->newlist[1] = list1[kn];
#endif
return kn;
}
int Position::evaluate_raw_make_list_diff()
{
const int sq_bk = SQ_BKING;
const int sq_wk = SQ_WKING;
const kkp_entry* ppkppb = kpp3[sq_bk];
const kkp_entry* ppkppw = kpp3[Inv(sq_wk)];
int score = kk[sq_bk][sq_wk];
for (int kn = PIECENUMBER_MIN; kn <= PIECENUMBER_MAX; kn++){
const int k0 = list0[kn];
const int k1 = list1[kn];
const short* pkppb = ppkppb[k0];
const short* pkppw = ppkppw[k1];
for (int j = PIECENUMBER_MIN; j < kn; j++){
const int l0 = list0[j];
const int l1 = list1[j];
score += pkppb[l0];
score -= pkppw[l1];
}
score += kkp[sq_bk][sq_wk][k0];
}
return score;
}
// 駒割り以外の全計算
// pos.st->BKPP,WKPP,KPPを初期化する。Position::set()で一度だけ呼び出される。(以降は差分計算)
// 手番側から見た評価値を返すので注意。(他の評価関数とは設計がこの点において異なる)
Value compute_eval(const Position& pos)
{
Square sq_bk = pos.king_square(BLACK);
Square sq_wk = pos.king_square(WHITE);
const auto* ppkppb = kpp[sq_bk];
const auto* ppkppw = kpp[Inv(sq_wk)];
auto& pos_ = *const_cast<Position*>(&pos);
auto list_fb = pos_.eval_list()->piece_list_fb();
auto list_fw = pos_.eval_list()->piece_list_fw();
int i, j;
BonaPiece k0, k1,l0,l1;
// 評価値の合計
EvalSum sum;
// SSE2は少なくとも有るという前提で。
// sum.p[0](BKPP)とsum.p[1](WKPP)をゼロクリア
sum.m[0] = _mm_setzero_si128();
// KK
sum.p[2] = kk[sq_bk][sq_wk];
for (i = 0; i < PIECE_NO_KING; ++i)
{
k0 = list_fb[i];
k1 = list_fw[i];
const auto* pkppb = ppkppb[k0];
const auto* pkppw = ppkppw[k1];
for (j = 0; j < i; ++j)
{
l0 = list_fb[j];
l1 = list_fw[j];
#if 0
sum.p[0] += pkppb[l0];
sum.p[1] += pkppw[l1];
#else
// SSEによる実装
// pkppw[l1][0],pkppw[l1][1],pkppb[l0][0],pkppb[l0][1]の16bit変数4つを整数拡張で32bit化して足し合わせる
__m128i tmp;
tmp = _mm_set_epi32(0, 0, *reinterpret_cast<const int32_t*>(&pkppw[l1][0]), *reinterpret_cast<const int32_t*>(&pkppb[l0][0]));
tmp = _mm_cvtepi16_epi32(tmp);
sum.m[0] = _mm_add_epi32(sum.m[0], tmp);
#endif
}
sum.p[2] += kkp[sq_bk][sq_wk][k0];
}
auto& info = *pos.state();
info.sum = sum;
sum.p[2][0] += pos.state()->materialValue * FV_SCALE;
return Value(sum.sum(pos.side_to_move()) / FV_SCALE);
}
static void Inv(const InputVecType& y, OutputVecType& x)
{
x = y;
for (size_t i = 0; i < y.n_elem; i++)
x(i) = Inv(y(i));
}
void Position::init_make_list()
{
memset(list0, 0, sizeof(list0));
memset(list1, 0, sizeof(list1));
memset(listkn, 0, sizeof(listkn));
memset(handcount, 0, sizeof(handcount));
for (PieceNumber kn = PIECENUMBER_MIN; kn <= PIECENUMBER_MAX; ++kn){
const int kpos = knpos[kn];
const Piece piece = Piece(knkind[kn]);
int count, sq;
switch (kpos) {
case 0:
break;
case 1: // 先手持駒
case 2: // 後手持駒
count = ++handcount[piece];
list0[kn] = NanohaTbl::HandIndex0[piece] + count;
list1[kn] = NanohaTbl::HandIndex1[piece] + count;
listkn[list0[kn]] = kn;
break;
default:
if ((SFU <= piece && piece <= SRY && piece != SOU) ||
(GFU <= piece && piece <= GRY && piece != GOU)) {
sq = conv_z2sq(kpos);
list0[kn] = NanohaTbl::KppIndex0[piece] + sq;
list1[kn] = NanohaTbl::KppIndex1[piece] + Inv(sq);
}
break;
}
}
}
bool GSpringDamperBody::applyForce(bool badd_)
{
if ( pLeftBody == NULL || pRightBody == NULL ) return false;
SE3 T = inv_T_left * Inv(pLeftBody->T_global) * pRightBody->T_global * T_right;
//se3 x(Log(T.GetRotation()), T.GetPosition()); // relative location of the right body from the left body
se3 x(Log(T)); // relative location of the right body from the left body
se3 V_left = Ad(inv_T_left, pLeftBody->V) - Ad(T * inv_T_right, pRightBody->V); // relative velocity of the left body
dse3 F_left; // force to be acting on the left body
for (int i=0; i<6; i++) {
F_left[i] = K[i] * x[i] - C[i] * V_left[i];
}
if ( badd_ ) {
pLeftBody->Fe += dAd(inv_T_left, F_left);
pRightBody->Fe += dAd(T * inv_T_right, -F_left);
} else {
pLeftBody->Fe -= dAd(inv_T_left, F_left);
pRightBody->Fe -= dAd(T * inv_T_right, -F_left);
}
return true;
}
// 評価値のスケール前の値を計算します。
int Position::evaluate_raw_correct() const
{
int list0[PIECENUMBER_MAX + 1]; //駒番号numのlist0
int list1[PIECENUMBER_MAX + 1]; //駒番号numのlist1
int nlist = make_list_correct(list0, list1);
const int sq_bk = SQ_BKING;
const int sq_wk = SQ_WKING;
const kkp_entry* ppkppb = kpp3[sq_bk];
const kkp_entry* ppkppw = kpp3[Inv(sq_wk)];
int score = kk[sq_bk][sq_wk];
for (int kn = 0; kn < nlist; kn++){
const int k0 = list0[kn];
const int k1 = list1[kn];
const int16_t* pkppb = ppkppb[k0];
const int16_t* pkppw = ppkppw[k1];
for (int j = 0; j < kn; j++){
score += pkppb[list0[j]];
score -= pkppw[list1[j]];
}
score += kkp[sq_bk][sq_wk][k0];
}
return score;
}
void Triangle::Collide(SecondaryContext &ctx, int idx, int first, int last) const {
Vec3q tnrm(plane.x, plane.y, plane.z);
Vec3q ta(a), tca(ca), tba(ba);
floatq zero(0.0f), one(1.0f), tit0(it0);
int count = last - first + 1;
for(int q = 0; q < count; q++) {
int tq = q + first;
const Vec3q dir = ctx.rayDir[tq];
floatq idet = Inv(dir | tnrm);
Vec3q tvec = ctx.rayOrigin[tq] - ta;
floatq dist = -(tvec | tnrm) * idet;
Vec3q tvec0 = tba ^ tvec;
Vec3q tvec1 = tvec ^ tca;
idet *= tit0;
floatq v = (dir | tvec0) * idet;
floatq u = (dir | tvec1) * idet;
f32x4b test = Min(u, v) >= zero && u + v <= one;
test = test && /*idet > zero &&*/ dist >= zero && dist < ctx.distance[tq];
ctx.distance[tq] = Condition(test, dist, ctx.distance[tq]);
ctx.normals[tq] = Condition(test, tnrm, ctx.normals[tq]);
ctx.triIds[tq] = Condition(i32x4b(test), idx, ctx.triIds[tq]);
ctx.barycentric[tq] = Condition(test, Vec2q(u, v), ctx.barycentric[tq]);
}
}
//int Position::make_list_apery(int list0[NLIST], int list1[NLIST], int nlist) const
int Position::make_list_apery(int list0[], int list1[], int nlist) const
{
static const struct {
int f_pt, e_pt;
} base_tbl[] = {
{-1 , -1 }, // 0:---
{f_pawn , e_pawn }, // 1:SFU
{f_lance , e_lance }, // 2:SKY
{f_knight, e_knight}, // 3:SKE
{f_silver, e_silver}, // 4:SGI
{f_gold , e_gold }, // 5:SKI
{f_bishop, e_bishop}, // 6:SKA
{f_rook , e_rook }, // 7:SHI
{-1 , -1 }, // 8:SOU
{f_gold , e_gold }, // 9:STO
{f_gold , e_gold }, // 10:SNY
{f_gold , e_gold }, // 11:SNK
{f_gold , e_gold }, // 12:SNG
{-1 , -1 }, // 13:--
{f_horse , e_horse }, // 14:SUM
{f_dragon, e_dragon}, // 15:SRY
{-1 , -1 }, // 16:---
{e_pawn , f_pawn }, // 17:GFU
{e_lance , f_lance }, // 18:GKY
{e_knight, f_knight}, // 19:GKE
{e_silver, f_silver}, // 20:GGI
{e_gold , f_gold }, // 21:GKI
{e_bishop, f_bishop}, // 22:GKA
{e_rook , f_rook }, // 23:GHI
{-1 , -1 }, // 24:GOU
{e_gold , f_gold }, // 25:GTO
{e_gold , f_gold }, // 26:GNY
{e_gold , f_gold }, // 27:GNK
{e_gold , f_gold }, // 28:GNG
{-1 , -1 }, // 29:---
{e_horse , f_horse }, // 30:GUM
{e_dragon, f_dragon} // 31:GRY
};
int sq;
// 駒番号：1〜2が玉、3〜40が玉以外
for (int kn = 3; kn <= 40; kn++) {
const int z = knpos[kn];
if (z < 0x11) continue; // 持ち駒除く
int piece = knkind[kn];
sq = conv_z2sq(z);
assert(piece <= GRY && sq < nsquare);
assert(base_tbl[piece].f_pt != -1);
list0[nlist] = base_tbl[piece].f_pt + sq;
list1[nlist] = base_tbl[piece].e_pt + Inv(sq);
nlist++;
}
assert( nlist == NLIST );
return nlist;
}
void DiagonalMatrixTemplate<T>::inplaceInverse()
{
if(this->empty())
FatalError(MatrixError_SizeZero);
ItT v=this->begin();
for(int i=0; i<this->n; i++,v++)
*v = Inv(*v);
}
Vec3q SATSampler::operator()(const Vec2q &uv,const Vec2q &diff) const {
f32x4b fullMask=diff.x>=0.5f||diff.x>= 0.5f;
if(ForAll(fullMask)) return Vec3q(avg.x,avg.y,avg.z);
Vec2q tDiff=diff*floatq(0.5f);
Vec2q a=(uv-tDiff),b=(uv+tDiff);
a*=Vec2q(floatq(w),floatq(h));
b*=Vec2q(floatq(w),floatq(h));
i32x4 ax(a.x),ay(a.y);
i32x4 bx(b.x),by(b.y);
ax&=wMask; ay&=hMask;
bx&=wMask; by&=hMask;
union { __m128 count; float countf[4]; };
TSample sum[4];
i32x4 one(1);
if(ForAll(ax<=bx&&ay<=by)) {
count = (f32x4(by-ay+one)*f32x4(bx-ax+one)).m;
ComputeRect(ax,ay,bx,by,sum);
}
else for(int k=0;k<4;k++) {
if(ax[k]>bx[k]) {
if(ay[k]>by[k]) {
countf[k]=(bx[k]+1)*(by[k]+1)+(w-ax[k])*(h-ay[k]);
sum[k]=ComputeRect(0,0,bx[k],by[k])+ComputeRect(ax[k],ay[k],w-1,h-1);
}
else {
countf[k]=(bx[k]+1+w-ax[k])*(by[k]-ay[k]+1);
sum[k]=ComputeRect(0,ay[k],bx[k],by[k])+ComputeRect(ax[k],ay[k],w-1,by[k]);
}
}
else {
if(ay[k]>by[k]) {
countf[k]=(bx[k]-ax[k]+1)*(by[k]+h+1-ay[k]);
sum[k]=ComputeRect(ax[k],0,bx[k],by[k])+ComputeRect(ax[k],ay[k],bx[k],h-1);
}
else {
countf[k]=(by[k]-ay[k]+1)*(bx[k]-ax[k]+1);
sum[k]=ComputeRect(ax[k],ay[k],bx[k],by[k]);
}
}
}
union {
__m128 out[3];
struct { float ox[4]; float oy[4]; float oz[4]; } o;
};
o.ox[0]=sum[0].R(); o.oy[0]=sum[0].G(); o.oz[0]=sum[0].B();
o.ox[1]=sum[1].R(); o.oy[1]=sum[1].G(); o.oz[1]=sum[1].B();
o.ox[2]=sum[2].R(); o.oy[2]=sum[2].G(); o.oz[2]=sum[2].B();
o.ox[3]=sum[3].R(); o.oy[3]=sum[3].G(); o.oz[3]=sum[3].B();
return Condition(fullMask,Vec3q(avg.x,avg.y,avg.z),
Vec3q(out[0], out[1], out[2]) * Inv(floatq(count) * 255.0f));
}
// 評価値の差分計算を行う
// 値がない時はfalseを返す。
bool Position::calc_difference(SearchStack* ss) const
{
if ((ss - 1)->staticEvalRaw == INT_MAX) { return false; }
int diff = 0;
const auto* ppkppb = kpp3[SQ_BKING];
const auto* ppkppw = kpp3[Inv(SQ_WKING)];
/* oldは引く。newは足す。
* 参照されてない＆２重に参照してるとこに注意
*/
// king-move
// TODO: 差分計算できるらしい
if (st->changeType == 0) { return false; }
// newlist
diff += doapc(st->newlist);
// oldlist
diff -= doapc(st->oldlist);
// newlist oldlist 引きすぎたので足す
diff += ppkppb[st->newlist[0]][st->oldlist[0]];
diff -= ppkppw[st->newlist[1]][st->oldlist[1]];
// cap
if (st->changeType == 2) { // newが２つ
// newcap oldlist 引きすぎたので足す
diff += ppkppb[st->newcap[0]][st->oldlist[0]];
diff -= ppkppw[st->newcap[1]][st->oldlist[1]];
// newcap
diff += doapc(st->newcap);
// newlist newcap (２回足されてるので引く)
diff -= ppkppb[st->newlist[0]][st->newcap[0]];
diff += ppkppw[st->newlist[1]][st->newcap[1]];
// oldcap
diff -= doapc(st->oldcap);
// new oldcap 引きすぎたので足す
diff += ppkppb[st->newlist[0]][st->oldcap[0]];
diff -= ppkppw[st->newlist[1]][st->oldcap[1]];
diff += ppkppb[st->newcap[0]][st->oldcap[0]];
diff -= ppkppw[st->newcap[1]][st->oldcap[1]];
// oldcap oldlist 参照されてない
diff -= ppkppb[st->oldcap[0]][st->oldlist[0]];
diff += ppkppw[st->oldcap[1]][st->oldlist[1]];
}
//else if (st->ct !=1 ){ MYABORT(); }
// セーブ
ss->staticEvalRaw = Value(diff) + (ss - 1)->staticEvalRaw;
return true;
}
// 評価関数が正しいかどうかを判定するのに使う
Value Position::evaluate_correct(const Color us) const
{
int list0[PIECENUMBER_MAX + 1]; //駒番号numのlist0
int list1[PIECENUMBER_MAX + 1]; //駒番号numのlist1
int nlist = make_list_correct(list0, list1);
const int sq_bk = SQ_BKING;
const int sq_wk = SQ_WKING;
const auto* ppkppb = Evaluater::KPP[sq_bk];
const auto* ppkppw = Evaluater::KPP[Inv(sq_wk)];
EvalSum score;
score.p[2] = Evaluater::KK[sq_bk][sq_wk];
#if defined USE_AVX2_EVAL || defined USE_SSE_EVAL
score.m[0] = _mm_setzero_si128();
for (int i = 0; i < nlist; ++i) {
const int k0 = list0[i];
const int k1 = list1[i];
const auto* pkppb = ppkppb[k0];
const auto* pkppw = ppkppw[k1];
for (int j = 0; j < i; ++j) {
const int l0 = list0[j];
const int l1 = list1[j];
__m128i tmp;
tmp = _mm_set_epi32(0, 0, *reinterpret_cast<const int32_t*>(&pkppw[l1][0]), *reinterpret_cast<const int32_t*>(&pkppb[l0][0]));
tmp = _mm_cvtepi16_epi32(tmp);
score.m[0] = _mm_add_epi32(score.m[0], tmp);
}
score.p[2] += Evaluater::KKP[sq_bk][sq_wk][k0];
}
#else
score.p[0][0] = 0;
score.p[0][1] = 0;
score.p[1][0] = 0;
score.p[1][1] = 0;
for (int i = 0; i < nlist; i++ ) {
const int k0 = list0[i];
const int k1 = list1[i];
assert(0 <= k0 && k0 < fe_end);
assert(0 <= k1 && k1 < fe_end);
const auto* pkppb = ppkppb[k0];
const auto* pkppw = ppkppw[k1];
for (int j = 0; j < i; j++ ) {
const int l0 = list0[j];
const int l1 = list1[j];
assert(0 <= l0 && l0 < fe_end);
assert(0 <= l1 && l1 < fe_end);
score.p[0] += pkppb[l0];
score.p[1] += pkppw[l1];
}
score.p[2] += Evaluater::KKP[sq_bk][sq_wk][k0];
}
#endif
score.p[2][0] += MATERIAL * FV_SCALE;
return Value(score.sum(us) / FV_SCALE);
}
/*得到该矩阵的逆，如可逆返回true和逆矩阵，如果不可逆则返回false，输入矩阵不变*/
BOHGE_FORCEINLINE bool GetInverse(Matrix22<T>& out) const
{
T det = this->CalculateDet();
if(true == Math::isZero(det))//如果矩阵的行列式为零则该矩阵没有逆，返回false，退出计算
{
return false;
}
Matrix22<T> Inv(a22, -a12, -a21, a11);//转置矩阵
out = Inv / det;//结果等于转置矩阵*矩阵的行列式
return true;
}
Value Position::evaluate(const Color us, SearchStack* ss)
{
const int sq_bk = SQ_BKING;
const int sq_wk = SQ_WKING;
assert(0 <= sq_bk && sq_bk < nsquare);
assert(0 <= sq_wk && sq_wk < nsquare);
const auto* ppkppb = Evaluater::KPP[sq_bk ];
const auto* ppkppw = Evaluater::KPP[Inv(sq_wk)];
EvalSum score;
score.p[2] = Evaluater::KK[sq_bk][sq_wk];
#if defined USE_AVX2_EVAL || defined USE_SSE_EVAL
score.m[0] = _mm_setzero_si128();
for (int kn = PIECENUMBER_MIN; kn <= PIECENUMBER_MAX; kn++) {
const int k0 = list0[kn];
const int k1 = list1[kn];
const auto* pkppb = ppkppb[k0];
const auto* pkppw = ppkppw[k1];
for (int j = PIECENUMBER_MIN; j < kn; j++) {
const int l0 = list0[j];
const int l1 = list1[j];
__m128i tmp;
tmp = _mm_set_epi32(0, 0, *reinterpret_cast<const int32_t*>(&pkppw[l1][0]), *reinterpret_cast<const int32_t*>(&pkppb[l0][0]));
tmp = _mm_cvtepi16_epi32(tmp);
score.m[0] = _mm_add_epi32(score.m[0], tmp);
}
score.p[2] += Evaluater::KKP[sq_bk][sq_wk][k0];
}
#else
score.p[0][0] = 0;
score.p[0][1] = 0;
score.p[1][0] = 0;
score.p[1][1] = 0;
for (int i = 0; i < nlist; i++ ) {
const int k0 = list0[i];
const int k1 = list1[i];
assert(0 <= k0 && k0 < fe_end);
assert(0 <= k1 && k1 < fe_end);
const auto* pkppb = ppkppb[k0];
const auto* pkppw = ppkppw[k1];
for (int j = 0; j < i; j++ ) {
const int l0 = list0[j];
const int l1 = list1[j];
assert(0 <= l0 && l0 < fe_end);
assert(0 <= l1 && l1 < fe_end);
score.p[0] += pkppb[l0];
score.p[1] += pkppw[l1];
}
score.p[2] += Evaluater::KKP[sq_bk][sq_wk][k0];
}
#endif
score.p[2][0] += MATERIAL * FV_SCALE;
return Value(score.sum(us) / FV_SCALE);
}
void DiagonalMatrixTemplate<T>::setInverse(const MyT& a)
{
if(this->empty())
resize(a.n);
else if(this->size() != a.size()) {
RaiseErrorFmt(WHERE_AM_I,MatrixError_IncompatibleDimensions,this->n,this->n,a.n,a.n);
}
ItT v=this->begin();
ItT va=a.begin();
for(int i=0; i<this->n; i++,v++,va++)
*v = Inv(*va);
}
static void LookAt(matrix_t row, const float x, const float y, const float z)
{
if(FnearZero(x) && FnearZero(z)) {
if (FnearZero(y)) {
MtxIdentity(row);
return;
} else {
row[0][0] = 1;
row[1][0] = 0;
row[2][0] = 0;
row[0][1] = 0;
row[1][1] = 0;
row[2][1] = (y<0) ? -1 : 1;
row[0][2] = 0;
row[1][2] = -row[2][1];
row[2][2] = 0;
}
} else {
const float x2 = x*x;
const float y2 = y*y;
const float z2 = z*z;
const float nxz = Sqrt(x2+z2);
const float oonxz = Inv(nxz);
const float oonxyz = ISqrt(x2+y2+z2);
{
const float tmp = oonxz*oonxyz;
row[0][1] = -y*x*tmp;
row[1][1] = nxz*oonxyz;
row[2][1] = -y*z*tmp;
}
{
const float tmp02 = x * oonxyz;
const float tmp22 = z * oonxyz;
row[0][2] = tmp02;
row[1][2] = y * oonxyz;
row[2][2] = tmp22;
{
float tmp = (nxz + y2*oonxz) * oonxyz;
row[0][0] = tmp22 * tmp;
row[2][0] = -tmp02 * tmp;
row[1][0] = 0;
}
}
}
row[0][3] = row[1][3] = row[2][3] = 0;
row[3][3] = 1;
}
// 駒割り以外の全計算
// pos.st->BKPP,WKPP,KPPを初期化する。Position::set()で一度だけ呼び出される。(以降は差分計算)
Value compute_eval(const Position& pos)
{
Square sq_bk0 = pos.king_square(BLACK);
Square sq_wk1 = Inv(pos.king_square(WHITE));
auto& pos_ = *const_cast<Position*>(&pos);
auto list_fb = pos_.eval_list()->piece_list_fb();
auto list_fw = pos_.eval_list()->piece_list_fw();
int i, j;
BonaPiece k0, k1;
int32_t sumBKPP, sumWKPP, sumKKP;
sumKKP = kkp[sq_bk0][sq_wk1][fe_end];
sumBKPP = 0;
sumWKPP = 0;
for (i = 0; i < PIECE_NO_KING; i++)
{
k0 = list_fb[i];
k1 = list_fw[i];
sumKKP += kkp[sq_bk0][sq_wk1][k0];
for (j = 0; j < i; j++)
{
sumBKPP += kpp[sq_bk0][k0][list_fb[j]];
sumWKPP -= kpp[sq_wk1][k1][list_fw[j]];
}
}
auto& info = *pos.state();
info.sumKKP = Value(sumKKP);
info.sumBKPP = Value(sumBKPP);
info.sumWKPP = Value(sumWKPP);
#ifdef USE_EHASH
// eval cacheに保存しておく。
EvalHash e;
e.sumKKP = Value(sumKKP);
e.sumBKPP = Value(sumBKPP);
e.sumWKPP = Value(sumWKPP);
e.key = info.key();
ehash[e.key & (EHASH_SIZE - 1)] = e;
#endif
// KKP配列の32bit化に伴い、KKP用だけ512倍しておく。(それくらいの計算精度はあるはず..)
// 最終的なKKP = sumKKP / (FV_SCALE * FV_SCALE_KKP)
return Value((sumBKPP + sumWKPP + sumKKP/ FV_SCALE_KKP)/FV_SCALE);
}
void Change(int x,int l,int r,int v)
{
if(l<=t[x].l&&r>=t[x].r&&(t[x].max[0]<=v||t[x].min[0]>v))
{
if(t[x].max[0]<=v) Inv(x);
return;
}
if(t[x].tag) push_down(x);
int mid=t[x].l+t[x].r>>1;
if(l<=mid) Change(t[x].ls,l,r,v);
if(r>mid) Change(t[x].rs,l,r,v);
push_up(x);
}
void MtxFrustum(matrix_t row, const float left, const float right,
const float top, const float bottom,
const float zNear, const float zFar)
{
const float twoNear = 2.0f * zNear;
const float ooRightMinusLeft = Inv(right - left);
const float ooTopMinusBottom = Inv(top - bottom);
const float ooFarMinusNear = Inv(zFar - zNear);
row[0][0] = twoNear * ooRightMinusLeft;
row[1][1] = twoNear * ooTopMinusBottom;
row[3][2] = twoNear * zFar * ooFarMinusNear;
row[2][0] = (right + left) * ooRightMinusLeft;
row[2][1] = (top + bottom) * ooTopMinusBottom;
row[2][2] = (zFar + zNear) * ooFarMinusNear;
row[0][1] = row[0][2] = row[0][3] =
row[1][0] = row[1][2] = row[1][3] =
row[3][0] = row[3][1] = row[3][3] = 0.0f;
row[2][3] = -1.0f;
}
// 差分計算
// index[2]は動かした駒のlist
int Position::doapc(const int index[2]) const
{
const int sq_bk = SQ_BKING;
const int sq_wk = SQ_WKING;
int sum = kkp[sq_bk][sq_wk][index[0]];
const auto* pkppb = kpp3[sq_bk][index[0]];
const auto* pkppw = kpp3[Inv(sq_wk)][index[1]];
for (int kn = PIECENUMBER_MIN; kn <= PIECENUMBER_MAX; kn++) {
sum += pkppb[list0[kn]];
sum -= pkppw[list1[kn]];
}
return sum;
}
Piano::Piano(Vettore *P1e,Vettore *P2e,Vettore *P3e){
P1.Copy(P1e);
P2.Copy(P2e);
P3.Copy(P3e);
Dir21 = P2 - P1;
Dir31 = P3 - P1;
Dir23 = P2 - P3;
for(int d=0;d<3;d++){
Dir21[d] = P2[d] - P1[d];
Dir31[d] = P3[d] - P1[d];
Dir23[d] = P2[d] - P3[d];
}
Vettore P4e = P1 + Dir21 + Dir31;
P4.Copy(&P4e);
Norm = Dir21 ^ Dir31;
Norm[0] = Dir21.x[1]*Dir31.x[2] - Dir21.x[2]*Dir31.x[1];
Norm[1] = Dir21.x[2]*Dir31.x[0] - Dir21.x[0]*Dir31.x[2];
Norm[2] = Dir21.x[0]*Dir31.x[1] - Dir21.x[1]*Dir31.x[0];
Norm.Normalize();
// printf("------\n");
// P1.Print();
// P2.Print();
// Dir21.Print();
// Norm.Print();
for(int d=0;d<3;d++){
if( fabs(Norm[d]) > 0.){
InvNorm[d] = 1./Norm[d];
IsInf[d] = 0;
}
else{
InvNorm[d] = 0.;
IsInf[d] = 1;
}
}
for(int d=0;d<3;d++){
Bound[d*2 ] = MIN(P1[d],MIN(P2[d],P3[d]));
//Bound[d*2 ] = MIN(P1[d],MIN(P2[d],MIN(P3[d],P4[d])));
Bound[d*2+1] = MAX(P1[d],MAX(P2[d],P3[d]));
//Bound[d*2+1] = MAX(P1[d],MAX(P2[d],MAX(P3[d],P4[d])));
}
dPar = - P1[0]*Norm[0] - P1[1]*Norm[1] - P1[2]*Norm[2];
mxy[0] = (P1[1] - P2[1])*Inv(P1[0] - P2[0]);
qxy[0] = P1[1] - mxy[0]*P1[0];
mxz[0] = (P1[2] - P2[2])*Inv(P1[0] - P2[0]);
qxz[0] = P1[2] - mxz[0]*P1[0];
mxy[1] = (P1[1] - P3[1])*Inv(P1[0] - P3[0]);
qxy[1] = P1[1] - mxy[1]*P1[0];
mxz[1] = (P1[2] - P3[2])*Inv(P1[0] - P3[0]);
qxz[1] = P1[2] - mxz[1]*P1[0];
mxy[2] = (P3[1] - P2[1])*Inv(P3[0] - P2[0]);
qxy[2] = P1[1] - mxy[2]*P1[0];
mxz[2] = (P3[2] - P2[2])*Inv(P3[0] - P2[0]);
qxz[2] = P1[2] - mxz[2]*P1[0];
Rad = 1.;
}
