void forces (std::vector<contact> conpnt[], int nb, iREAL * linear[3], iREAL mass[], int parmat[], iREAL * mparam[NMAT],
int pairnum, int pairs[], int ikind[], iREAL * iparam[NINT])
{
for(int i=0;i<nb;i++)
{
iREAL vi[3];
vi[0] = linear[0][i];
vi[1] = linear[1][i];
vi[2] = linear[2][i];
for(unsigned int k = 0; k<conpnt[i].size(); k++)
{
iREAL p[3], n[3], vj[3], vij[3];
p[0] = conpnt[i][k].point[0];
p[1] = conpnt[i][k].point[1];
p[2] = conpnt[i][k].point[2];
n[0] = conpnt[i][k].normal[0];
n[1] = conpnt[i][k].normal[1];
n[2] = conpnt[i][k].normal[2];
int j = conpnt[i][k].pid[1]; //get index from slave body contact
vj[0] = linear[0][j];
vj[1] = linear[1][j];
vj[2] = linear[2][j];
SUB (vj, vi, vij); // relative linear velocity
int ij = pairing (pairnum, pairs, conpnt[i][k].color[0], conpnt[i][k].color[1]);//get material from colours
iREAL f[3];
switch (ikind[ij])
{
case GRANULAR:
granular (n, vij, conpnt[i][k].depth, i, j, mass, iparam, ij, f);
break;
default:
printf ("ERROR: invalid pairing kind");
break;
}
if(conpnt[i][k].pid[0] == i)
{
conpnt[i][k].force[0] = f[0];
conpnt[i][k].force[1] = f[1];
conpnt[i][k].force[2] = f[2];
}
else
{
conpnt[i][k].force[0] = -f[0];
conpnt[i][k].force[1] = -f[1];
conpnt[i][k].force[2] = -f[2];
}
}
}
}
// calculator
void CoordinationBase::calculate()
{
double ncoord=0.;
Tensor virial;
vector<Vector> deriv(getNumberOfAtoms());
// deriv.resize(getPositions().size());
if(nl->getStride()>0 && invalidateList){
nl->update(getPositions());
}
unsigned stride=comm.Get_size();
unsigned rank=comm.Get_rank();
if(serial){
stride=1;
rank=0;
}else{
stride=comm.Get_size();
rank=comm.Get_rank();
}
for(unsigned int i=rank;i<nl->size();i+=stride) { // sum over close pairs
Vector distance;
unsigned i0=nl->getClosePair(i).first;
unsigned i1=nl->getClosePair(i).second;
if(getAbsoluteIndex(i0)==getAbsoluteIndex(i1)) continue;
if(pbc){
distance=pbcDistance(getPosition(i0),getPosition(i1));
} else {
distance=delta(getPosition(i0),getPosition(i1));
}
double dfunc=0.;
ncoord += pairing(distance.modulo2(), dfunc,i0,i1);
deriv[i0] = deriv[i0] + (-dfunc)*distance ;
deriv[i1] = deriv[i1] + dfunc*distance ;
virial=virial+(-dfunc)*Tensor(distance,distance);
}
if(!serial){
comm.Sum(ncoord);
if(!deriv.empty()) comm.Sum(&deriv[0][0],3*deriv.size());
comm.Sum(virial);
}
for(unsigned i=0;i<deriv.size();++i) setAtomsDerivatives(i,deriv[i]);
setValue (ncoord);
setBoxDerivatives (virial);
}
//---------------------------------------------------------------------------//
TEUCHOS_UNIT_TEST( SplineInterpolationPairing, dim_3_test )
{
int dim = 3;
int num_src_points = 10;
int num_src_coords = dim*num_src_points;
Teuchos::Array<double> src_coords(num_src_coords);
for ( int i = 0; i < num_src_points; ++i )
{
src_coords[dim*i] = 1.0*i;
src_coords[dim*i+1] = 1.0;
src_coords[dim*i+2] = 1.0;
}
int num_tgt_points = 2;
int num_tgt_coords = dim*num_tgt_points;
Teuchos::Array<double> tgt_coords( num_tgt_coords );
tgt_coords[0] = 4.9;
tgt_coords[1] = 1.0;
tgt_coords[2] = 1.0;
tgt_coords[3] = 10.0;
tgt_coords[4] = 1.0;
tgt_coords[5] = 1.0;
double radius = 1.1;
DataTransferKit::SplineInterpolationPairing<3> pairing(
src_coords(), tgt_coords(), radius );
Teuchos::ArrayView<const unsigned> view = pairing.childCenterIds( 0 );
TEST_EQUALITY( 3, view.size() );
TEST_EQUALITY( 5, view[0] )
TEST_EQUALITY( 4, view[1] )
TEST_EQUALITY( 6, view[2] )
view = pairing.childCenterIds( 1 );
TEST_EQUALITY( 1, view.size() );
TEST_EQUALITY( 9, view[0] );
Teuchos::ArrayRCP<std::size_t> children_per_parent =
pairing.childrenPerParent();
TEST_EQUALITY( children_per_parent[0], 3 );
TEST_EQUALITY( children_per_parent[1], 1 );
}
void forces (std::vector<contact> conpnt[],
int nb, int pid[], iREAL * position[3], iREAL * angular[6], iREAL * linear[3],
iREAL mass[], iREAL *force[3], iREAL *torque[3], iREAL gravity[3], int parmat[], iREAL * mparam[NMAT],
int pairnum, int pairs[], int ikind[], iREAL * iparam[NINT])
{
for (int i = 0; i < nb; i++)
{
iREAL oi[3], v[3], x[3];
oi[0] = angular[3][i];
oi[1] = angular[4][i];
oi[2] = angular[5][i];
v[0] = linear[0][i];
v[1] = linear[1][i];
v[2] = linear[2][i];
x[0] = position[0][i];
x[1] = position[1][i];
x[2] = position[2][i];
/* update contact forces */
for(unsigned int k = 0; k<conpnt[i].size(); k++)
{
iREAL p[3], n[3], z[3], vi[3], vj[3], oj[3], vij[3], oij[3];
p[0] = conpnt[i][k].point[0];
p[1] = conpnt[i][k].point[1];
p[2] = conpnt[i][k].point[2];
z[0] = p[0]-x[0];
z[1] = p[1]-x[1];
z[2] = p[2]-x[2];
vi[0] = oi[1]*z[2]-oi[2]*z[1] + v[0];
vi[1] = oi[2]*z[0]-oi[0]*z[2] + v[1];
vi[2] = oi[0]*z[1]-oi[1]*z[0] + v[2];
int j = conpnt[i][k].pid[1]; //get index from slave body contact
z[0] = p[0]-position[0][j];
z[1] = p[1]-position[1][j];
z[2] = p[2]-position[2][j];
oj[0] = angular[3][j];
oj[1] = angular[4][j];
oj[2] = angular[5][j];
vj[0] = oj[1]*z[2]-oj[2]*z[1] + linear[0][j];
vj[1] = oj[2]*z[0]-oj[0]*z[2] + linear[1][j];
vj[2] = oj[0]*z[1]-oj[1]*z[0] + linear[2][j];
SUB (vj, vi, vij); // relative linear velocity
SUB (oj, oi, oij); // relative angular velocity
int ij = pairing (pairnum, pairs, conpnt[i][k].color[0], conpnt[i][k].color[1]);//get material from colours
iREAL f[3];
switch (ikind[ij])
{
case GRANULAR:
granular_force (conpnt[i][k].normal, vij, oij, conpnt[i][k].depth, i, j, mass, iparam, ij, f);
break;
case BONDED:
/* TODO */
break;
case UFORCE:
/* TODO */
break;
default:
printf ("ERROR: invalid pairing kind");
break;
}
iREAL a[3];
a[0] = conpnt[i][j].point[0]-x[0];//boundary
a[1] = conpnt[i][j].point[1]-x[1];
a[2] = conpnt[i][j].point[2]-x[2];
//master force
force[0][i] += f[0];
force[1][i] += f[1];
force[2][i] += f[2];
torque[0][i] += a[1]*f[2] - a[2]*f[1];//cross product
torque[1][i] += a[2]*f[0] - a[0]*f[2];
torque[2][i] += a[0]*f[1] - a[1]*f[0];
//add force to slaves
f[0] = -f[0];
f[1] = -f[1];
f[2] = -f[2];
force[0][j] += f[0];
force[1][j] += f[1];
force[2][j] += f[2];
a[0] = conpnt[i][j].point[0]-position[0][j];//boundary
a[1] = conpnt[i][j].point[1]-position[1][j];
a[2] = conpnt[i][j].point[2]-position[2][j];
torque[0][j] += a[1]*f[2] - a[2]*f[1];//cross product
torque[1][j] += a[2]*f[0] - a[0]*f[2];
torque[2][j] += a[0]*f[1] - a[1]*f[0];
}
}
for(int i=0;i<nb;i++)
{
force[0][i] = force[0][i] + mass[i] * gravity[0];
force[1][i] = force[1][i] + mass[i] * gravity[1];
force[2][i] = force[2][i] + mass[i] * gravity[2];
printf("Total Force of body: %i is: %f %f %f\n", i, force[0][i], force[1][i], force[2][i]);
printf("Total Torque of body: %i is: %f %f %f\n", i, torque[0][i], torque[1][i], torque[2][i]);
std::vector<contact>().swap(conpnt[i]);
}
}
// calculator
void CoordinationBase::calculate()
{
double ncoord=0.;
Tensor virial;
vector<Vector> deriv(getNumberOfAtoms());
// deriv.resize(getPositions().size());
if(nl->getStride()>0 && invalidateList) {
nl->update(getPositions());
}
unsigned stride=comm.Get_size();
unsigned rank=comm.Get_rank();
if(serial) {
stride=1;
rank=0;
} else {
stride=comm.Get_size();
rank=comm.Get_rank();
}
unsigned nt=OpenMP::getNumThreads();
const unsigned nn=nl->size();
if(nt*stride*10>nn) nt=nn/stride/10;
if(nt==0)nt=1;
#pragma omp parallel num_threads(nt)
{
std::vector<Vector> omp_deriv(getPositions().size());
Tensor omp_virial;
#pragma omp for reduction(+:ncoord) nowait
for(unsigned int i=rank; i<nn; i+=stride) {
Vector distance;
unsigned i0=nl->getClosePair(i).first;
unsigned i1=nl->getClosePair(i).second;
if(getAbsoluteIndex(i0)==getAbsoluteIndex(i1)) continue;
if(pbc) {
distance=pbcDistance(getPosition(i0),getPosition(i1));
} else {
distance=delta(getPosition(i0),getPosition(i1));
}
double dfunc=0.;
ncoord += pairing(distance.modulo2(), dfunc,i0,i1);
Vector dd(dfunc*distance);
Tensor vv(dd,distance);
if(nt>1) {
omp_deriv[i0]-=dd;
omp_deriv[i1]+=dd;
omp_virial-=vv;
} else {
deriv[i0]-=dd;
deriv[i1]+=dd;
virial-=vv;
}
}
#pragma omp critical
if(nt>1) {
for(int i=0; i<getPositions().size(); i++) deriv[i]+=omp_deriv[i];
virial+=omp_virial;
}
}
if(!serial) {
comm.Sum(ncoord);
if(!deriv.empty()) comm.Sum(&deriv[0][0],3*deriv.size());
comm.Sum(virial);
}
for(unsigned i=0; i<deriv.size(); ++i) setAtomsDerivatives(i,deriv[i]);
setValue (ncoord);
setBoxDerivatives (virial);
}
bool command_extra(uint8_t code)
{
uint32_t t;
uint16_t b;
switch (code) {
case KC_H:
case KC_SLASH: /* ? */
print("\n\n----- Bluetooth RN-42 Help -----\n");
print("i: RN-42 info\n");
print("b: battery voltage\n");
print("Del: enter/exit RN-42 config mode\n");
print("Slck: RN-42 initialize\n");
print("1-4: restore link\n");
print("F1-F4: store link\n");
print("p: pairing\n");
if (config_mode) {
return true;
} else {
print("u: toggle Force USB mode\n");
return false; // to display default command help
}
case KC_P:
pairing();
return true;
/* Store link address to EEPROM */
case KC_F1:
store_link(RN42_LINK0);
return true;
case KC_F2:
store_link(RN42_LINK1);
return true;
case KC_F3:
store_link(RN42_LINK2);
return true;
case KC_F4:
store_link(RN42_LINK3);
return true;
/* Restore link address to EEPROM */
case KC_1:
restore_link(RN42_LINK0);
return true;
case KC_2:
restore_link(RN42_LINK1);
return true;
case KC_3:
restore_link(RN42_LINK2);
return true;
case KC_4:
restore_link(RN42_LINK3);
return true;
case KC_5:
xprintf("blah! \n");
//rn42_cts_hi();
if (stay_connected == 1){
dprintf("Disconnect after 5 min.\n");
stay_connected = !stay_connected;
}else{
dprintf("Stay connected.\n");
stay_connected = 1;
//last_press_timer = timer_read32();
}
return true;
case KC_6:
clear_keyboard();
host_set_driver(&rn42_config_driver); // null driver; not to send a key to host
rn42_disconnect();
return true;
case KC_I:
print("\n----- RN-42 info -----\n");
xprintf("protocol: %s\n", (host_get_driver() == &rn42_driver) ? "RN-42" : "LUFA");
xprintf("force_usb: %X\n", force_usb);
xprintf("rn42: %s\n", rn42_rts() ? "OFF" : (rn42_linked() ? "CONN" : "ON"));
xprintf("rn42_autoconnecting(): %X\n", rn42_autoconnecting());
xprintf("config_mode: %X\n", config_mode);
xprintf("USB State: %s\n",
(USB_DeviceState == DEVICE_STATE_Unattached) ? "Unattached" :
(USB_DeviceState == DEVICE_STATE_Powered) ? "Powered" :
(USB_DeviceState == DEVICE_STATE_Default) ? "Default" :
(USB_DeviceState == DEVICE_STATE_Addressed) ? "Addressed" :
(USB_DeviceState == DEVICE_STATE_Configured) ? "Configured" :
(USB_DeviceState == DEVICE_STATE_Suspended) ? "Suspended" : "?");
xprintf("battery: ");
switch (battery_status()) {
case FULL_CHARGED: xprintf("FULL"); break;
case CHARGING: xprintf("CHARG"); break;
case DISCHARGING: xprintf("DISCHG"); break;
case LOW_VOLTAGE: xprintf("LOW"); break;
default: xprintf("?"); break;
};
xprintf("\n");
xprintf("RemoteWakeupEnabled: %X\n", USB_Device_RemoteWakeupEnabled);
xprintf("VBUS: %X\n", USBSTA&(1<<VBUS));
t = timer_read32()/1000;
uint8_t d = t/3600/24;
uint8_t h = t/3600;
uint8_t m = t%3600/60;
uint8_t s = t%60;
xprintf("uptime: %02u %02u:%02u:%02u\n", d, h, m, s);
xprintf("LINK0: %s\r\n", get_link(RN42_LINK0));
xprintf("LINK1: %s\r\n", get_link(RN42_LINK1));
xprintf("LINK2: %s\r\n", get_link(RN42_LINK2));
xprintf("LINK3: %s\r\n", get_link(RN42_LINK3));
return true;
case KC_B:
// battery monitor
t = timer_read32()/1000;
b = battery_voltage();
xprintf("BAT: %umV\t", b);
xprintf("%02u:", t/3600);
xprintf("%02u:", t%3600/60);
xprintf("%02u\n", t%60);
return true;
case KC_U:
if (config_mode) return false;
if (force_usb) {
print("Auto mode\n");
force_usb = false;
} else {
print("USB mode\n");
clear_keyboard();
host_set_driver(&rn42_driver);
}
return true;
case KC_DELETE:
/* RN-42 Command mode */
if (rn42_autoconnecting()) {
enter_command_mode();
command_state = CONSOLE;
config_mode = true;
} else {
exit_command_mode();
command_state = ONESHOT;
config_mode = false;
}
return true;
case KC_SCROLLLOCK:
init_rn42();
return true;
default:
if (config_mode)
return true;
else
return false; // yield to default command
}
return true;
}
vector<SplitEdge> eulerCSplit(const vector<SplitEdge> &g, vector<SplitEdge> &B, int s, unsigned int k, vector<vector<int>> X, historyIndex &h)
{
vector<SplitEdge> GHat = g;
GHat = fixEquation4(GHat, s);
if (cG(s, g) % 2 != 0)
{
cout << "ERROR: odd CG(s)" << endl;
throw logic_error("");
}
vector<vector<int>> X1 = X;
while (X1.size() >= 3)
{
X1 = sortByCG(GHat, s, X1);
vector<int> cgsX1;
for (unsigned i = 0; i < X1.size(); i++)
{
cgsX1.push_back(cG(s, X1[i], GHat));
}
int del1P = 0;
int del2P = 0;
if ((cG(s, X1[0], GHat) - cG(s, X1[1], GHat)) >= cG(s, X1[X1.size() - 1], GHat))
del1P = cG(s, X1[X1.size() - 1], GHat);
else
{
del2P = (int)ceil(double(cG(s, X1[X1.size() - 1], GHat) - cG(s, X1[0], GHat) + cG(s, X1[1], GHat)) * .5);
del1P = cG(s, X1[X1.size() - 1], GHat) - del2P;
}
GHat = removeZeroWeighted(GHat);
GHat = pairing(X1[0], X1[X1.size() - 1], del1P, B, GHat, s, h);
GHat = removeZeroWeighted(GHat);
GHat = pairing(X1[1], X1[X1.size() - 1], del2P, B, GHat, s, h);
bool erase1 = cG(s, X1[0], GHat) == 0;
bool erase2 = cG(s, X1[1], GHat) == 0;
bool eraseLast = cG(s, X1.back(), GHat) == 0;
vector<vector<int>> X11;
if (!erase1)
X11.push_back(X1[0]);
if (!erase2)
X11.push_back(X1[1]);
for (unsigned i = 2; i < X1.size() - 1; i++)
X11.push_back(X1[i]);
if (!eraseLast)
X11.push_back(X1.back());
X1 = X11;
if (X1.size() == 1)
{
cout << "ERROR: X1.size() == 1" << endl;
throw logic_error("");
}
}
int del12 = 0;
if (X1.size() != 0)
{
if (cG(s, X1[0], GHat) != cG(s, X1[1], GHat))
{
cout << "eulerCSplit sanity check failure. Make sure cG(X1) != cG(X2)" << endl;
throw logic_error("");
}
del12 = cG(s, X1[0], GHat);
GHat = removeZeroWeighted(GHat);
GHat = pairing(X1[0], X1[1], del12, B, GHat, s, h);
}
return GHat;
}
