// overriden from class cProxyPointForceAlgo
cVector3d ch_proxyPointForceAlgo::computeForcesD(const cVector3d& a_toolPos, const cVector3d& a_toolVel)
{
// update device position
m_deviceGlobalPos = a_toolPos;
// check if world has been defined; if so, compute forces
if (m_world != NULL)
{
// compute next best position of proxy
computeNextBestProxyPosition(m_deviceGlobalPos, a_toolVel);
// update proxy to next best position
m_proxyGlobalPos = m_nextBestProxyGlobalPos;
// compute force vector applied to device
updateForce();
// return result
return (m_lastGlobalForce);
}
// if no world has been defined in which algorithm operates, there is no force
else
{
return (cVector3d(0.0, 0.0, 0.0));
}
}
void TpsTestPeSurf2dMD::verlet(TpsTestPeSurf2dTimeslice& m)
{
double dt2 = 0.5 *_dt;
m.v[0] += dt2 * m.f[0];
m.v[1] += dt2 * m.f[1];
m.x[0] += _dt * m.v[0];
m.x[1] += _dt * m.v[1];
updateForce();
m.v[0] += dt2* m.f[0];
m.v[1] += dt2* m.f[1];
return;
}
// update〜系関数をまとめる
void update(float bar_move) {
// もし失敗していたら
if(miss) {
// 落下させる
setForce(0, 0.3);
}
// 失敗していない場合は
else {
// バーの動きと逆方向に力がかかる（すべるような動き）
setForce(-bar_move, 0);
updateForce();
}
updateVelocity();
updatePos();
}
void Foam::sixDoFRigidBodyMotion::updateForce
(
const pointField& positions,
const vectorField& forces,
scalar deltaT
)
{
vector fGlobal = vector::zero;
vector tauGlobal = vector::zero;
if (Pstream::master())
{
fGlobal = sum(forces);
forAll(positions, i)
{
tauGlobal += (positions[i] - centreOfMass()) ^ forces[i];
}
}
updateForce(fGlobal, tauGlobal, deltaT);
}
// 力の更新と座標の更新をupdateとしてまとめる
void Particle::update(){
updateForce();
updatePos();
}
// Computes acceleration to every control point of the jello cube,
// which is in state given by 'jello'.
// Returns result in array 'a'.
void computeAcceleration(struct world * jello, struct point a[8][8][8])
{
// for you to implement ...
int x = 0, y = 0, z = 0; // Index variables for looping through all 512 points
point typeP, curP; // typeP = one of the 32 nodes to which the curP point is connected
int collided = 0;
memset( (void*)a, 0, sizeof(a));
memset( (void*)&typeP, 0, sizeof(typeP));
memset( (void*)&curP, 0, sizeof(curP));
memset( (void*)force, 0, sizeof(force));
// FORCE FIELD COMPUTATION
if(ActivateFF)
{
// Check for force field
if(jello->resolution)
{
// compute the force field cube size and store it in the global variable
forceFieldCubeSize = ((double)4)/jello->resolution;
noOfCubesInRow = (int)(((double)jello->resolution) / forceFieldCubeSize);
// Compute the effect of force field and update the forces due to it on all the nodes
computeForceField(jello);
}
}
// COLLISION DETECTION AND RESPONSE
for(z = 0; z < 8; z++)
{
for(y = 0; y < 8; y++)
{
for(x = 0; x < 8; x++)
{
memset( (void*)&curP, 0, sizeof(curP));
curP.x = x;
curP.y = y;
curP.z = z;
// Checks whether any of the nodes is colliding with the bounding box and Sphere
checkForCollision(curP, jello);
}
}
}
for(z = 0; z < 8; z++)
{
for(y = 0; y < 8; y++)
{
for(x = 0; x < 8; x++)
{
curP.x = x;
curP.y = y;
curP.z = z;
// Check for all the springs possible = 32
// SAME HORIZONTAL LAYER
//-----------------------------------------------------------------
// TYPE 1 -> (x+1, y, z) structural spring
typeP.x = x + 1;
typeP.y = y;
typeP.z = z;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 2 -> (x-1, y, z)
typeP.x = x - 1;
typeP.y = y;
typeP.z = z;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 3 -> (x, y, z+1)
typeP.x = x;
typeP.y = y;
typeP.z = z + 1;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 4 -> (x, y, z-1)
typeP.x = x;
typeP.y = y;
typeP.z = z - 1;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 5 -> (x+1, y, z+1)
typeP.x = x + 1;
typeP.y = y;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 6 -> (x+1, y, z-1)
typeP.x = x + 1;
typeP.y = y;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 7 -> (x-1, y, z+1)
typeP.x = x - 1;
typeP.y = y;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 8 -> (x-1, y, z-1)
typeP.x = x - 1;
typeP.y = y;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// BOTTOM LAYER
//-----------------------------------------------------------------
// TYPE 9 -> (x+1, y+1, z)
typeP.x = x + 1;
typeP.y = y + 1;
typeP.z = z;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 10 -> (x-1, y+1, z)
typeP.x = x - 1;
typeP.y = y + 1;
typeP.z = z;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 11 -> (x, y+1, z+1)
typeP.x = x;
typeP.y = y + 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 12 -> (x, y+1, z-1)
typeP.x = x;
typeP.y = y + 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 13 -> (x+1, y+1, z+1)
typeP.x = x + 1;
typeP.y = y + 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 14 -> (x+1, y+1, z-1)
typeP.x = x + 1;
typeP.y = y + 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 15 -> (x-1, y+1, z+1)
typeP.x = x - 1;
typeP.y = y + 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 16 -> (x-1, y+1, z-1)
typeP.x = x - 1;
typeP.y = y + 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 17 -> (x, y+1, z)
typeP.x = x;
typeP.y = y + 1;
typeP.z = z;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TOP LAYER
// TYPE 18 -> (x+1, y-1, z)
typeP.x = x + 1;
typeP.y = y - 1;
typeP.z = z;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 19 -> (x-1, y-1, z)
typeP.x = x - 1;
typeP.y = y - 1;
typeP.z = z;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 20 -> (x, y-1, z+1)
typeP.x = x;
typeP.y = y - 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 21 -> (x, y-1, z-1)
typeP.x = x;
typeP.y = y - 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 22 -> (x+1, y-1, z+1)
typeP.x = x + 1;
typeP.y = y - 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 23 -> (x+1, y-1, z-1)
typeP.x = x + 1;
typeP.y = y - 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 24 -> (x-1, y-1, z+1)
typeP.x = x - 1;
typeP.y = y - 1;
typeP.z = z + 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 25 -> (x-1, y-1, z-1)
typeP.x = x - 1;
typeP.y = y - 1;
typeP.z = z - 1;
updateForce(curP, typeP, jello, SHEARDIAGONAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 26 -> (x, y-1, z)
typeP.x = x;
typeP.y = y - 1;
typeP.z = z;
updateForce(curP, typeP, jello, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// BEND SPRINGS
//-----------------------------------------------------------------
// TYPE 27 -> (x+2, y, z)
typeP.x = x + 2;
typeP.y = y;
typeP.z = z;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 28 -> (x-2, y, z)
typeP.x = x - 2;
typeP.y = y;
typeP.z = z;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 29 -> (x, y+2, z)
typeP.x = x;
typeP.y = y + 2;
typeP.z = z;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 30 -> (x, y-2, z)
typeP.x = x;
typeP.y = y - 2;
typeP.z = z;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 31 -> (x, y, z+2)
typeP.x = x;
typeP.y = y;
typeP.z = z + 2;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 32 -> (x, y, z-2)
typeP.x = x;
typeP.y = y;
typeP.z = z - 2;
updateForce(curP, typeP, jello, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// Check if the point obtained in this type is valid -> inside cube check
// blah blah
// check if the point obtained in this type is already parsed -> check for the x,y,z loop comparison
// blah blah
// if not parsed then find the hooks force and damping force.
// add the force value to both the end points, i.e; current looping point and the type obtained point
} // for indexX
} // for indexY
} // for indexZ
memset( (void*)a, 0, sizeof(a));
for(z = 0; z < 8; z++)
{
for(y = 0; y < 8; y++)
{
for(x = 0; x < 8; x++)
{
a[x][y][z].x = force[x][y][z].x / jello->mass ;
a[x][y][z].y = force[x][y][z].y / jello->mass ;
a[x][y][z].z = force[x][y][z].z / jello->mass ;
}
}
}
}
void TriParticle::update(){
resetForce();
updateForce();
updatePos();
rot += rotVel;
}
// Computes acceleration to every point of the cloth
void computeAcceleration(cloth *clothItem)
{
node typeP, curP; // typeP = one of the 12 nodes to which the curP point is connected
int collided = 0, index;
memset( (void*)&typeP, 0, sizeof(typeP));
memset( (void*)&curP, 0, sizeof(curP));
/*
// FORCE FIELD COMPUTATION
if(ActivateFF)
{
// Check for force field
if(jello->resolution)
{
// compute the force field cube size and store it in the global variable
forceFieldCubeSize = ((double)4)/jello->resolution;
noOfCubesInRow = (int)(((double)jello->resolution) / forceFieldCubeSize);
// Compute the effect of force field and update the forces due to it on all the nodes
computeForceField(jello);
}
}
*/
// COLLISION DETECTION AND RESPONSE
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
memset( (void*)&curP, 0, sizeof(curP));
index = FindIndexInArray(curP.y, curP.x, clothItem->width);
pCPY(vMake(0.0), clothItem->force[index]);
curP.x = col;
curP.y = row;
// Checks whether any of the nodes is colliding with the bounding box and Sphere
checkForCollision(curP, clothItem);
}
}
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
curP.x = col;
curP.y = row;
// Check for all the springs possible = 12
// STRUCTURAL SPRINGS
//-----------------------------------------------------------------
// TYPE 1 -> (x+1, y, z) structural spring
typeP.x = col + 1;
typeP.y = row;
updateForce(curP, typeP, clothItem, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 2 -> (x-1, y, z)
typeP.x = col - 1;
typeP.y = row;
updateForce(curP, typeP, clothItem, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 3 -> (x, y+1, z)
typeP.x = col;
typeP.y = row + 1;
updateForce(curP, typeP, clothItem, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 4 -> (x, y-1, z)
typeP.x = col;
typeP.y = row - 1;
updateForce(curP, typeP, clothItem, STRUCTURAL);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// SHEAR SPRINGS
//-----------------------------------------------------------------
// TYPE 5 -> (x+1, y+1, z)
typeP.x = col + 1;
typeP.y = row + 1;
updateForce(curP, typeP, clothItem, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 6 -> (x-1, y+1, z)
typeP.x = col - 1;
typeP.y = row + 1;
updateForce(curP, typeP, clothItem, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 7 -> (x+1, y-1, z)
typeP.x = col + 1;
typeP.y = row - 1;
updateForce(curP, typeP, clothItem, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 8 -> (x-1, y-1, z)
typeP.x = col - 1;
typeP.y = row - 1;
updateForce(curP, typeP, clothItem, SHEARSIDE);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// BEND SPRINGS
//-----------------------------------------------------------------
// TYPE 9 -> (x+2, y, z)
typeP.x = col + 2;
typeP.y = row;
updateForce(curP, typeP, clothItem, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 10 -> (x-2, y, z)
typeP.x = col - 2;
typeP.y = row;
updateForce(curP, typeP, clothItem, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 11 -> (x, y+2, z)
typeP.x = col;
typeP.y = row + 2;
updateForce(curP, typeP, clothItem, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
// TYPE 12 -> (x, y-2, z)
typeP.x = col;
typeP.y = row - 2;
updateForce(curP, typeP, clothItem, BEND);
memset( (void*)&typeP, 0, sizeof(typeP));
//-----------------------------------------------------------------
}// for col
}// for row
for(int row = 0; row < clothItem->height; row++)
{
for(int col = 0; col < clothItem->width; col++)
{
index = FindIndexInArray(row, col, clothItem->width);
clothItem->acceleration[index].x = clothItem->force[index].x / clothItem->mass;
clothItem->acceleration[index].y = clothItem->force[index].y / clothItem->mass;
clothItem->acceleration[index].z = clothItem->force[index].z / clothItem->mass;
}
}
}
