void Rigidbody::Update(float dt) {
Transform *transform = _entity->GetTransform();
Vec2 v = vel * dt;
Vector<RayPointData> rays;
float eps = 0.01f;
for (int i = 0; i < _xPoints + 2; i++) {
float x = transform->pos.x + eps + (transform->size.x - 2 * eps) * i / (_xPoints + 1);
if (v.y <= 0.0f) {
rays.push_back({ { x, transform->pos.y }, { 0.0f, v.y }, CD_Up });
}
if (v.y >= 0.0f) {
CollisionDirection dir = CD_Down;
if (i == 0) {
dir = (CollisionDirection)(dir | CD_DownLeftSide);
}
else if (i == _xPoints + 1) {
dir = (CollisionDirection)(dir | CD_DownRightSide);
}
rays.push_back({ { x, transform->pos.y + transform->size.y }, { 0.0f, v.y }, dir });
}
}
for (int i = 0; i < _yPoints + 2; i++) {
float y = transform->pos.y + eps + (transform->size.y - 2 * eps) * i / (_yPoints + 1);
if (v.x <= 0.0f) {
rays.push_back({ { transform->pos.x, y }, { v.x, 0.0f }, CD_Left });
}
if (v.x >= 0.0f) {
rays.push_back({ { transform->pos.x + transform->size.x, y }, { v.x, 0.0f }, CD_Right });
}
}
Collider *collider = GetComponent<Collider>();
_collidedDirs = CD_None;
static int frame = 0;
frame++;
for (auto r : rays) {
float dist = FLT_MAX;
if (collider->Raycast(r.pos, r.pos + r.dir, &dist, _layerMask)) {
_collidedDirs = (CollisionDirection)(_collidedDirs | r.colDir);
if (r.colDir & CD_Horizontal) {
v.x = absMin(v.x, dist * sign1(v.x));
}
else {
v.y = absMin(v.y, dist * sign1(v.y));
}
}
}
if (_collidedDirs & CD_Vertical) {
vel.y = 0.0f;
}
if (!(_collidedDirs & CD_Down)) {
vel.y += kGravity * dt;
}
transform->pos += v;
}
int main(int argc, char* argv[]) {
int a = 0;
for (a = -5; a < 5; a++) {
printf("%d\n", sign1(a));
}
return 0;
}
bool bp_script_sign(struct bp_keystore *ks, const cstring *fromPubKey,
const struct bp_tx *txTo, unsigned int nIn,
int nHashType)
{
if (!txTo || !txTo->vin || nIn >= txTo->vin->len)
return false;
struct bp_txin *txin = parr_idx(txTo->vin, nIn);
/* get signature hash */
bu256_t hash;
bp_tx_sighash(&hash, fromPubKey, txTo, nIn, nHashType);
/* match fromPubKey against templates, to find what pubkey[hashes]
* are required for signing
*/
struct bscript_addr addrs;
if (!bsp_addr_parse(&addrs, fromPubKey->str, fromPubKey->len))
return false;
cstring *scriptSig = cstr_new_sz(64);
bool rc = false;
bu160_t key_id;
struct buffer *kbuf;
/* sign, based on script template matched above */
switch (addrs.txtype) {
case TX_PUBKEY:
kbuf = addrs.pub->data;
bu_Hash160((unsigned char *)&key_id, kbuf->p, kbuf->len);
if (!sign1(&key_id, ks, &hash, nHashType, scriptSig))
goto out;
break;
case TX_PUBKEYHASH:
kbuf = addrs.pubhash->data;
memcpy(&key_id, kbuf->p, kbuf->len);
if (!sign1(&key_id, ks, &hash, nHashType, scriptSig))
goto out;
if (!bkeys_pubkey_append(ks, &key_id, scriptSig))
goto out;
break;
case TX_SCRIPTHASH: /* TODO; not supported yet */
case TX_MULTISIG:
goto out;
case TX_NONSTANDARD: /* unknown script type, cannot sign */
goto out;
}
if (txin->scriptSig)
cstr_free(txin->scriptSig, true);
txin->scriptSig = scriptSig;
scriptSig = NULL;
rc = true;
out:
if (scriptSig)
cstr_free(scriptSig, true);
bsp_addr_free(&addrs);
return rc;
}
void Distance_Function_RHS (UserCtx *user, Vec Levelset_RHS, int wall_distance)
{
DA da = user->da, fda = user->fda;
DALocalInfo info = user->info;
PetscInt xs, xe, ys, ye, zs, ze;
PetscInt mx, my, mz;
PetscInt i, j, k;
PetscReal dpdc, dpde, dpdz;
Vec Csi = user->lCsi, Eta = user->lEta, Zet = user->lZet;
Vec Aj = user->lAj;
Cmpnts ***csi, ***eta, ***zet;
PetscReal ***aj, ***level, ***level0, ***rhs, ***grad_level;
PetscInt lxs, lys, lzs, lxe, lye, lze;
PetscReal ***nvert;
Vec L, lLevelset0; // grad level, level0
xs = info.xs; xe = xs + info.xm;
ys = info.ys; ye = ys + info.ym;
zs = info.zs; ze = zs + info.zm;
lxs = xs; lxe = xe;
lys = ys; lye = ye;
lzs = zs; lze = ze;
mx = info.mx; my = info.my; mz = info.mz;
if (xs==0) lxs = xs+1;
if (ys==0) lys = ys+1;
if (zs==0) lzs = zs+1;
if (xe==mx) lxe = xe-1;
if (ye==my) lye = ye-1;
if (ze==mz) lze = ze-1;
VecSet(Levelset_RHS, 0);
VecDuplicate(user->lP, &L);
VecDuplicate(user->lP, &lLevelset0);
DAGlobalToLocalBegin(user->da, LevelSet0, INSERT_VALUES, lLevelset0);
DAGlobalToLocalEnd(user->da, LevelSet0, INSERT_VALUES, lLevelset0);
DAVecGetArray(fda, Csi, &csi);
DAVecGetArray(fda, Eta, &eta);
DAVecGetArray(fda, Zet, &zet);
DAVecGetArray(da, Aj, &aj);
DAVecGetArray(da, user->lNvert, &nvert);
DAVecGetArray(da, user->lLevelset, &level);
DAVecGetArray(da, lLevelset0, &level0);
DAVecGetArray(da, Levelset_RHS, &rhs);
DAVecGetArray(da, L, &grad_level);
for (k=lzs; k<lze; k++)
for (j=lys; j<lye; j++)
for (i=lxs; i<lxe; i++) {
double dldc, dlde, dldz;
double dl_dx, dl_dy, dl_dz;
double csi0=csi[k][j][i].x,csi1=csi[k][j][i].y, csi2=csi[k][j][i].z;
double eta0=eta[k][j][i].x,eta1=eta[k][j][i].y, eta2=eta[k][j][i].z;
double zet0=zet[k][j][i].x,zet1=zet[k][j][i].y, zet2=zet[k][j][i].z;
double ajc = aj[k][j][i];
double dx=pow(1./aj[k][j][i],1./3.);
if(dthick_set) dx = dthick;
double sgn = sign1(level0[k][j][i], dx);
//if(wall_distance) sgn = sign(level0[k][j][i]);
//Compute_dlevel_center_levelset (i, j, k, mx, my, mz, sgn, wall_distance, level, nvert, &dldc, &dlde, &dldz);
Compute_dlevel_center_levelset (i, j, k, mx, my, mz, sign(level0[k][j][i]), wall_distance, level, nvert, &dldc, &dlde, &dldz); //100521
Compute_dscalar_dxyz (csi0, csi1, csi2, eta0, eta1, eta2, zet0, zet1, zet2, ajc, dldc, dlde, dldz, &dl_dx, &dl_dy, &dl_dz);
grad_level[k][j][i] = sqrt( dl_dx*dl_dx + dl_dy*dl_dy + dl_dz*dl_dz );
if(nvert[k][j][i]>0.1) grad_level[k][j][i]=0;
}
DAVecRestoreArray(da, L, &grad_level);
DALocalToLocalBegin(user->da, L, INSERT_VALUES, L);
DALocalToLocalEnd(user->da, L, INSERT_VALUES, L);
DAVecGetArray(da, L, &grad_level);
// Neumann, periodic conditions
if(xs==0 || xe==mx) {
int from, to;
for (k=lzs; k<lze; k++)
for (j=lys; j<lye; j++) {
if(xs==0) {
i = 1, from = i, to = 0;
if(i_periodic) from = mx-2;
else if(ii_periodic) from = -2;
grad_level[k][j][to] = grad_level[k][j][from];
}
if(xe==mx) {
i = mx-2, from = i, to = mx-1;
if(i_periodic) from = 1;
else if(ii_periodic) from = mx+1;
grad_level[k][j][to] = grad_level[k][j][from];
}
}
}
if(ys==0 || ye==my) {
int from, to;
for (k=lzs; k<lze; k++)
for (i=lxs; i<lxe; i++) {
if(ys==0) {
j = 1, from = j, to = 0;
if(j_periodic) from = my-2;
else if(jj_periodic) from = -2;
grad_level[k][to][i] = grad_level[k][from][i];
}
if(ye==my) {
j = my-2, from = j, to = my-1;
if(j_periodic) from = 1;
else if(jj_periodic) from = my+1;
grad_level[k][to][i] = grad_level[k][from][i];
}
}
}
if(zs==0 || ze==mz) {
int from, to;
for (j=lys; j<lye; j++)
for (i=lxs; i<lxe; i++) {
if(zs==0) {
k = 1, from = k, to = 0;
if(k_periodic) from = mz-2;
else if(kk_periodic) from = -2;
grad_level[to][j][i] = grad_level[from][j][i];
}
if(ze==mz) {
k = mz-2, from = k, to = mz-1;
if(k_periodic) from = 1;
else if(kk_periodic) from = mz+1;
grad_level[to][j][i] = grad_level[from][j][i];
}
}
}
DAVecRestoreArray(da, L, &grad_level);
DALocalToLocalBegin(user->da, L, INSERT_VALUES, L);
DALocalToLocalEnd(user->da, L, INSERT_VALUES, L);
DAVecGetArray(da, L, &grad_level);
for(k=zs; k<ze; k++)
for(j=ys; j<ye; j++)
for(i=xs; i<xe; i++) {
if (i<= 0 || i>= mx-1 || j<=0 || j>=my-1 || k<=0 || k>=mz-1 || nvert[k][j][i]>1.1){
rhs[k][j][i]=0.;
continue;
}
if(nvert[k][j][i]>0.1) {
rhs[k][j][i]=0.;
continue;
}
if(wall_distance) {
if(nvert[k][j][i]>0.1) { rhs[k][j][i]=0.; continue; }
if(i <= 1 && (user->bctype[0]==1 || user->bctype[0]==-1 || user->bctype[0]==-2)) { rhs[k][j][i]=0.; continue; }
if(i >= mx-2 && (user->bctype[1]==1 || user->bctype[1]==-1 || user->bctype[1]==-2)) { rhs[k][j][i]=0.; continue; }
if(j <=1 && (user->bctype[2]==1 || user->bctype[2]==-1 || user->bctype[2]==-2)) { rhs[k][j][i]=0.; continue; }
if(j >=my-2 && (user->bctype[3]==1 || user->bctype[3]==-1 || user->bctype[3]==-2 || user->bctype[3]==12)) { rhs[k][j][i]=0.; continue; }
if(k<=1 && (user->bctype[4]==1 || user->bctype[4]==-1 || user->bctype[4]==-2)) { rhs[k][j][i]=0.; continue; }
if(k>=mz-2 && (user->bctype[5]==1 || user->bctype[5]==-1 || user->bctype[5]==-2)){ rhs[k][j][i]=0.; continue; }
}
else if( !wall_distance && user->bctype[4]==5 && user->bctype[5]==4) {
//if ( fix_inlet && k==1 ) { rhs[k][j][i] = 0; continue; }
//haha if ( fix_outlet && k==mz-2 ) { rhs[k][j][i] = 0; continue; } // important to stabilize outlet
}
double dx=pow(1./aj[k][j][i],1./3.);
if(dthick_set) dx = dthick;
double sgn = sign1(level0[k][j][i],dx);
//if(wall_distance) sgn = sign(level0[k][j][i]);
double denom[3][3][3], num[3][3][3], weight[3][3][3];
for(int p=-1; p<=1; p++)
for(int q=-1; q<=1; q++)
for(int r=-1; r<=1; r++) {
int R=r+1, Q=q+1, P=p+1;
int K=k+r, J=j+q, I=i+p;
double phi = level[K][J][I], grad = grad_level[K][J][I], dx=pow(1./aj[K][J][I],1./3.);
if(dthick_set) dx = dthick;
double f = dH(phi,dx) * grad;
double _sgn = sign1( level0[K][J][I], dx );
//if(wall_distance) _sgn = sign(level0[K][J][I]);
num[R][Q][P] = dH(phi,dx) * _sgn * ( 1. - grad );
denom[R][Q][P] = dH(phi,dx) * f;
}
for(int p=-1; p<=1; p++)
for(int q=-1; q<=1; q++)
for(int r=-1; r<=1; r++) {
int R=r+1, Q=q+1, P=p+1;
int K=k+r, J=j+q, I=i+p;
if( (!i_periodic && !ii_periodic && (I==0 || I==mx-1 ) ) ||
(!j_periodic && !jj_periodic && (J==0 || J==my-1 ) ) ||
(!k_periodic && !kk_periodic && (K==0 || K==mz-1) ) ||
nvert[K][J][I]>0.1) {
num[R][Q][P] = num[1][1][1];
denom[R][Q][P] = denom[1][1][1];
}
}
get_weight (i, j, k, mx, my, mz, aj, nvert, 0.1, weight);
double numerator = integrate_testfilter(num, weight);
double denominator = integrate_testfilter(denom, weight);
double correction;
if( fabs(denominator)<1.e-10 ) correction=0;
else {
double grad = grad_level[k][j][i];
double phi = level[k][j][i];
double dx=pow(1./aj[k][j][i],1./3.);
if(dthick_set) dx = dthick;
double f = dH(phi,dx) * grad;
correction = - numerator / denominator;
correction *= dH(phi,dx) * grad;
}
double dlevel_dx, dlevel_dy, dlevel_dz;
double dldc, dlde, dldz;
double csi0 = csi[k][j][i].x, csi1 = csi[k][j][i].y, csi2 = csi[k][j][i].z;
double eta0 = eta[k][j][i].x, eta1 = eta[k][j][i].y, eta2 = eta[k][j][i].z;
double zet0 = zet[k][j][i].x, zet1 = zet[k][j][i].y, zet2 = zet[k][j][i].z;
double ajc = aj[k][j][i];
//Compute_dlevel_center_levelset (i, j, k, mx, my, mz, sgn, wall_distance, level, nvert, &dldc, &dlde, &dldz);
Compute_dlevel_center_levelset (i, j, k, mx, my, mz, sign(level0[k][j][i]), wall_distance, level, nvert, &dldc, &dlde, &dldz); //100521
Compute_dscalar_dxyz (csi0, csi1, csi2, eta0, eta1, eta2, zet0, zet1, zet2, ajc, dldc, dlde, dldz, &dlevel_dx, &dlevel_dy, &dlevel_dz);
rhs[k][j][i] = sgn * ( 1. - sqrt( dlevel_dx*dlevel_dx + dlevel_dy*dlevel_dy + dlevel_dz*dlevel_dz ) );
if(nvert[k][j][i+1]+nvert[k][j][i-1]+nvert[k][j+1][i]+nvert[k][j-1][i]+nvert[k+1][j][i]+nvert[k-1][j][i]>0.1) {
// correction = 0;
}
if(!wall_distance) rhs[k][j][i] += correction; // Sussman Fetami
}
DAVecRestoreArray(da, L, &grad_level);
DAVecRestoreArray(fda, Csi, &csi);
DAVecRestoreArray(fda, Eta, &eta);
DAVecRestoreArray(fda, Zet, &zet);
DAVecRestoreArray(da, Aj, &aj);
DAVecRestoreArray(da, user->lNvert, &nvert);
DAVecRestoreArray(da, user->lLevelset, &level);
DAVecRestoreArray(da, lLevelset0, &level0);
DAVecRestoreArray(da, Levelset_RHS, &rhs);
VecDestroy(L);
VecDestroy(lLevelset0);
}
