Matrix2f sampleJacobian(const PosType &x_) {
//check bounds
if(!insideBounds(x_))
return Matrix2f().setZero();
DataType dx, dy, vHi, vLo;
PosType pHi, pLo;
{
//dx derivative
pHi[0] = x_[0] + mHalfScaleFactor[0]; pHi[1] = x_[1];
pLo[0] = x_[0] - mHalfScaleFactor[0]; pLo[1] = x_[1];
vHi = sample(pHi),
vLo = sample(pLo);
if(!insideBounds(pHi)) {
//backward difference
dx = sample(x_);
dx = (dx - vLo) / mHalfScaleFactor[0];
} else if(!insideBounds(pLo)) {
//forward difference
dx = sample(x_);
dx = (vHi - dx) / mHalfScaleFactor[0];
} else {
//central difference
dx = (vHi - vLo) / (2*mHalfScaleFactor[0]);
}
}
{
//dy derivative
pHi[0] = x_[0]; pHi[1] = x_[1] + mHalfScaleFactor[1];
pLo[0] = x_[0]; pLo[1] = x_[1] - mHalfScaleFactor[1];
vHi = sample(pHi),
vLo = sample(pLo);
if(!insideBounds(pHi)) {
//backward difference
dy = sample(x_);
dy = (dy - vLo) / mHalfScaleFactor[1];
} else if(!insideBounds(pLo)) {
//forward difference
dy = sample(x_);
dy = (vHi - dy) / mHalfScaleFactor[1];
} else {
//central difference
dy = (vHi - vLo) / (2*mHalfScaleFactor[1]);
}
}
Matrix2f out;
out[0][0] = dx[0]; out[0][1] = dx[1];
out[1][0] = dy[0]; out[1][1] = dy[1];
return out;
}
Vector2f sampleGradient(const PosType &x_) {
//check bounds
if(!insideBounds(x_))
return PosType().setZero();
float32 dx, dy, vHi, vLo;
PosType pHi, pLo;
{
//dx derivative
pHi[0] = x_[0] + mHalfScaleFactor[0]; pHi[1] = x_[1];
pLo[0] = x_[0] - mHalfScaleFactor[0]; pLo[1] = x_[1];
vHi = sampleScalar(pHi),
vLo = sampleScalar(pLo);
if(!insideBounds(pHi)) {
//backward difference
dx = sampleScalar(x_);
dx = (dx - vLo) / mHalfScaleFactor[0];
} else if(!insideBounds(pLo)) {
//forward difference
dx = sampleScalar(x_);
dx = (vHi - dx) / mHalfScaleFactor[0];
} else {
//central difference
dx = (vHi - vLo) / (2*mHalfScaleFactor[0]);
}
}
{
//dy derivative
pHi[0] = x_[0]; pHi[1] = x_[1] + mHalfScaleFactor[1];
pLo[0] = x_[0]; pLo[1] = x_[1] - mHalfScaleFactor[1];
vHi = sampleScalar(pHi),
vLo = sampleScalar(pLo);
if(!insideBounds(pHi)) {
//backward difference
dy = sampleScalar(x_);
dy = (dy - vLo) / mHalfScaleFactor[1];
} else if(!insideBounds(pLo)) {
//forward difference
dy = sampleScalar(x_);
dy = (vHi - dy) / mHalfScaleFactor[1];
} else {
//central difference
dy = (vHi - vLo) / (2*mHalfScaleFactor[1]);
}
}
PosType out; out[0] = dx; out[1] = dy;
return out;
}
/*
* checks that a new shape's location is not overlapping another shape
* breakpoints and print outs warn against outside the walls locations
*/
bool Shape::locationError(int index, bool allow_one)
{
float shapeT,shapeB,shapeL,shapeR;
if(outsideBounds_x(LWALL,RWALL))
printf("left or right error [%i] (%f, %f) --- (%f, %f)\n",index,getLocation_x(),getLocation_y(),getVelocity_x(),getVelocity_y());
if(outsideBounds_y(TWALL,BWALL))
printf("top or bottom error [%i] (%f, %f) --- (%f, %f)\n",index,getLocation_x(),getLocation_y(),getVelocity_x(),getVelocity_y());
int count = 0;
for(int i=0; i<index; i++){
shapeL = Shape::board[i]->getLocation_x();
shapeR = shapeL + Shape::board[i]->getWidth();
shapeT = Shape::board[i]->getLocation_y();
shapeB = shapeT + Shape::board[i]->getHeight();
if(insideBounds(shapeL, shapeR, shapeT, shapeB)){
count++;
if((allow_one)&&(count<2))continue;
return true;
}
}
return false;
}
/*
* compares all other shape locations with that of itself to see if it will collide
*/
void Shape::checkCollision(int index)
{ float shapeT,shapeB,shapeL,shapeR;
for(int i=0; i<10; i++){
if(i==index)continue;
shapeL = Shape::board[i]->getLocation_x();
shapeR = shapeL + Shape::board[i]->getWidth();
shapeT = Shape::board[i]->getLocation_y();
shapeB = shapeT + Shape::board[i]->getHeight();
// check for shapes overlapping
if(insideBounds(shapeL, shapeR, shapeT, shapeB)){
//if the collision is already playing - stop it
if(shape_sounds[COLLISION_SOUND].getIsPlaying())
{
//stop the sound
shape_sounds[COLLISION_SOUND].stop();
//printf("Stopping the sound\n");
}
hit_count++;
Shape::board[i]->hit_count++;
//play a collision sound
// printf("COLLISION!\n");
//shape_sounds[COLLISION_SOUND].play();
// printf("Play Collision! \n");
collision_Bounce(i);
/***** this is where we can put switch collisions *****/
}
}
}
typename Field2<NUMCOMPONENTS>::DataType
Field2<NUMCOMPONENTS>::sample(const PosType &x_) const {
assert(mGridData.data.size());
//check bounds
if(!insideBounds(x_))
return DataType().setZero();
//get uniform parametrization location
PosType localPos;
localPos[0] = (x_[0] - mGridData.boundMin[0]) * mInvScaleFactor[0];
localPos[1] = (x_[1] - mGridData.boundMin[1]) * mInvScaleFactor[1];
//get grid indices
const card32 i0 = card32(std::floor(localPos[0])),
j0 = card32(std::floor(localPos[1]));
const card32 i1 = min(mGridData.dims[0]-1, i0+1),
j1 = min(mGridData.dims[1]-1, j0+1);
//get uniform parametrization
const float32 u = localPos[0] - float32(i0),
v = localPos[1] - float32(j0);
//perform bilinear interpolation for each data component
const float32 b00 = (1-u)*(1-v),
b10 = u*(1-v),
b01 = (1-u)*v,
b11 = u*v;
DataType out;
for(size_t k=0; k<NUMCOMPONENTS; ++k)
out[k] = b00 * mGridData.data[NUMCOMPONENTS*linearIndex(i0, j0) + k] +
b10 * mGridData.data[NUMCOMPONENTS*linearIndex(i1, j0) + k] +
b01 * mGridData.data[NUMCOMPONENTS*linearIndex(i0, j1) + k] +
b11 * mGridData.data[NUMCOMPONENTS*linearIndex(i1, j1) + k];
return out;
}
