void System::ROTATE_MOLECULE(double degree_amount, VECTOR direction,int Mol){
/**
\param[in] degree_amount The magnitude of rotation, in degrees
\param[in] direction The vector definining the axis of rotation. The magnitude of this vector does not matter.
\param[in] Mol The ID (not index!) of the molecule to be rotated
Simplest manipulation
Rotates Molecule with molecule id "int Mol" on amount of "double amount"
around the axis given by "VECTOR direction"
*/
MATRIX3x3 R;
double phi = M_PI*degree_amount/180.0; // Convert to the radians
double cs = cos(0.5*phi);
double si = sin(0.5*phi);
VECTOR u = direction.unit();
QUATERNION quat(cs,si*u.x,si*u.y,si*u.z);
QUATERNION_TO_MATRIX(quat,R);
int v = get_molecule_index_by_molecule_id(Mol);
if(v!=-1){
Molecules[v].Molecule_RB.Rotate(R);
rotate_atoms_of_molecule(v,R);
}
}
void System::ROTATE_FRAGMENT(double degree_amount, VECTOR direction,int Gr){
/**
\param[in] degree_amount The magnitude of rotation, in degrees
\param[in] direction The vector definining the axis of rotation. The magnitude of this vector does not matter.
\param[in] Gr The ID (not index!) of the group/fragment to be rotated
Simplest manipulation
Rotates the fragment with the fragment ID "int Gr" on amount of "double amount"
around the axis given by "VECTOR direction" around the fragment's center of mass
*/
MATRIX3x3 R;
double phi = M_PI*degree_amount/180.0; // Convert to the radians
double cs = cos(0.5*phi);
double si = sin(0.5*phi);
VECTOR u = direction.unit();
QUATERNION quat(cs,si*u.x,si*u.y,si*u.z);
QUATERNION_TO_MATRIX(quat,R);
int v = get_fragment_index_by_fragment_id(Gr);
if(v!=-1){
Fragments[v].Group_RB.Rotate(R);
rotate_atoms_of_fragment(v,R);
update_atoms_for_fragment(v);
}
// Molecule orientation does not change because the center of mass of the
// fragment v does not change
}
void System::TRANSLATE_FRAGMENT(double amount,VECTOR direction,int Fr){
/**
\param[in] amount The magnitude of the translation
\param[in] direction The vector definining the direction of the translation. The magnitude of this vector does not matter.
\param[in] Fr The ID (not index!) of the fragment to be translated
Simplest manipulation
Translates Fragment with fragment id "int Fr" on amount of "double amount"
in direction of "VECTOR direction"
*/
int v;
VECTOR displ = amount * direction.unit();
v = get_fragment_index_by_fragment_id(Fr);
if(v!=-1){
int mol_indx = Fragments[v].globMolecule_Index;
RigidBody& gtop = Fragments[v].Group_RB;
RigidBody& mtop = Molecules[mol_indx].Molecule_RB;
gtop.shift_position(displ);
mtop.shift_position((gtop.rb_mass/mtop.rb_mass)*displ);
update_atoms_for_fragment(v);
}// if v!=-1
}
void System::TRANSLATE_ATOM(double amount,VECTOR direction,int At){
/**
\param[in] amount The magnitude of translation
\param[in] direction The vector definining the direction of the translation. The magnitude of this vector does not matter.
\param[in] At The ID (not index!) of the atom to be translated
Simplest manipulation
Translates atom with atom id "int At" on amount of "double amount"
in direction of "VECTOR direction"
*/
int v;
VECTOR displ = amount * direction.unit();
v = get_atom_index_by_atom_id(At);
if(v!=-1){
int grp_indx = Atoms[v].globGroup_Index;
int mol_indx = Atoms[v].globMolecule_Index;
RigidBody& atop = Atoms[v].Atom_RB;
RigidBody& gtop = Fragments[grp_indx].Group_RB;
RigidBody& mtop = Molecules[mol_indx].Molecule_RB;
atop.shift_position(displ);
gtop.shift_position((atop.rb_mass/gtop.rb_mass)*displ);
mtop.shift_position((atop.rb_mass/mtop.rb_mass)*displ);
}// if v!=-1
}
/*
--------------------------------------------------------------------------------------------------
- check collision with stairs
--------------------------------------------------------------------------------------------------
*/
bool PlanesPhysicHandler::ColisionWithStair(const AABB & actorBB, const VECTOR &Speed, VECTOR &ModifiedSpeed)
{
float moveX = Speed.x;
float moveZ = Speed.z;
// calculate norm of speed
VECTOR speedNorm = Speed.unit();
float startX = (actorBB.P.x+actorBB.E.x)/2.0f;
float startZ = (actorBB.P.z+actorBB.E.z)/2.0f;
std::vector<StairPlane>::const_iterator it = _stairs.begin();
std::vector<StairPlane>::const_iterator end = _stairs.end();
// for each stairs
for(int i=0; it != end; ++it, ++i)
{
// project point to plane and check if we cross it
float DotProduct=speedNorm.dot(it->Normal);
// Determine If Ray Parallel To Plane
if (abs(DotProduct) > 0.000001f)
{
// Find Distance To Collision Point
float l2=(it->Normal.dot(it->C1-VECTOR(startX, actorBB.P.y, startZ)))/DotProduct;
// Test If Collision Behind Start or after end
if (l2 > 0 && l2 < Speed.length())
{
float collionsX = startX + (speedNorm.x * l2);
float collionsZ = startZ + (speedNorm.z * l2);
if((collionsX >= it->minX) && (collionsX <= it->maxX))
{
if((collionsZ >= it->minZ) && (collionsZ <= it->maxZ))
{
VECTOR spmY(Speed.x, 0, Speed.z);
VECTOR Vt(it->Normal.dot(spmY)*it->Normal);
VECTOR Vn(spmY - Vt);
ModifiedSpeed = Vn;
return true;
}
}
}
}
}
return false;
}
void RigidBody::Rotate(double phi,VECTOR& dir){
/**
\brief Arbitrary rotation in body frame, parameterized by a rotation axis and angle
The rotation axis and angle define a quaternion, which is then applied to produce rotation
\param[in] phi the rotation angle
\param[in] dir the rotation axis, direction
*/
double cs = cos(0.5*phi);
double si = sin(0.5*phi);
VECTOR u = dir.unit();
QUATERNION quat(cs,si*u.x,si*u.y,si*u.z);
Rotate(quat);
}
void System::TRANSLATE_MOLECULE(double amount,VECTOR direction,int Mol){
/**
\param[in] amount The magnitude of the translation
\param[in] direction The vector definining the direction of the translation. The magnitude of this vector does not matter.
\param[in] Mol The ID (not index!) of the molecule to be translated
Simplest manipulation
Translates Molecule with molecule id "int Mol" on amount of "double amount"
in direction of "VECTOR direction"
*/
int v;
VECTOR displ = amount * direction.unit();
v = get_molecule_index_by_molecule_id(Mol);
if(v!=-1){
int mol_indx = Fragments[v].globMolecule_Index;
RigidBody& mtop = Molecules[mol_indx].Molecule_RB;
mtop.shift_position(displ);
update_atoms_for_molecule(v);
}// if v!=-1
}
/*
--------------------------------------------------------------------------------------------------
- check collision with wall Z
--------------------------------------------------------------------------------------------------
*/
bool PlanesPhysicHandler::ColisionWithWallZ(const AABB & actorBB, const VECTOR &Speed,
float &ModifiedSpeedZ, float &ModifiedSpeedY)
{
float moveZ = Speed.z;
if(moveZ == 0)
return false;
// calculate norm of speed
VECTOR speedNorm = Speed.unit();
std::vector<int> tovisit;
float startZ = 0;
// if we go upward
if(moveZ > 0)
{
startZ = actorBB.E.z;
int stZ = (int)(startZ-0.0001f)+1; // max actor Z
int stopZ = (int)(startZ+moveZ);
for(; stZ <= stopZ; ++stZ)
tovisit.push_back(stZ);
}
else // if we go backward
{
startZ = actorBB.P.z;
int stZ = (int)startZ; // min actor Z
float tmpZ = startZ+moveZ;
int stopZ = (int)(tmpZ-0.0001f)+1;
if(tmpZ < 0)
stopZ = 0;
if(stopZ < 0)
stopZ = 0;
for(; stZ >= stopZ; --stZ)
tovisit.push_back(stZ);
}
for(size_t i=0; i<tovisit.size(); ++i)
{
const std::vector<NormalPlane> & vecfloor = _wallsZ[tovisit[i]];
std::vector<NormalPlane>::const_iterator it = vecfloor.begin();
std::vector<NormalPlane>::const_iterator end = vecfloor.end();
// keep memory of the last checked layer
float lastcheckedLayer = -1;
Square2D memorysquare;
float distance;
// check until the planes beeing after the actor move
for(; it != end; ++it)
{
if(it->Layer != lastcheckedLayer)
{
lastcheckedLayer = (float)it->Layer;
distance = (lastcheckedLayer-startZ) / speedNorm.z;
float offsetx = speedNorm.x * distance;
float offsety = speedNorm.y * distance;
memorysquare._minX = (actorBB.P.x + offsetx);
memorysquare._maxX = (actorBB.E.x + offsetx);
memorysquare._minZ = (actorBB.P.y + offsety);
memorysquare._maxZ = (actorBB.E.y + offsety);
}
if(it->Square.Overlap(memorysquare))
{
ModifiedSpeedZ = (lastcheckedLayer - startZ);
ModifiedSpeedZ += (ModifiedSpeedZ > 0) ? -0.0001f : 0.0001f;
ModifiedSpeedY *= distance;
return true;
}
}
}
return false;
}
/*
--------------------------------------------------------------------------------------------------
- check collision with floor
--------------------------------------------------------------------------------------------------
*/
bool PlanesPhysicHandler::PointColisionWithFloor(const AABB & actorBB, const VECTOR &Speed,
float &ModifiedSpeedY, bool &water)
{
float moveY = Speed.y;
if(moveY == 0)
return false;
// calculate norm of speed
VECTOR speedNorm = Speed.unit();
std::vector<int> tovisit;
float startY = 0;
// if we go upward
if(moveY > 0)
{
startY = actorBB.E.y;
int stY = (int)(startY-0.0001f)+1; // max actor Y
int stopY = (int)(startY+moveY);
for(; stY <= stopY; ++stY)
tovisit.push_back(stY);
}
else // if we go backward
{
startY = actorBB.P.y;
int stY = (int)startY; // min actor Y
float tmpY = startY+moveY;
int stopY = (int)(tmpY-0.0001f)+1;
if(tmpY < 0)
stopY = 0;
if(stopY < 0)
stopY = 0;
for(; stY >= stopY; --stY)
tovisit.push_back(stY);
}
for(size_t i=0; i<tovisit.size(); ++i)
{
const std::vector<NormalPlane> & vecfloor = _floors[tovisit[i]];
std::vector<NormalPlane>::const_iterator it = vecfloor.begin();
std::vector<NormalPlane>::const_iterator end = vecfloor.end();
// keep memory of the last checked layer
float lastcheckedLayer = -1;
float ptx, ptz;
// check until the planes beeing after the actor move
for(; it != end; ++it)
{
if(it->Layer != lastcheckedLayer)
{
lastcheckedLayer = (float)it->Layer;
float distance = (lastcheckedLayer-startY) / speedNorm.y;
ptx = (actorBB.P.x + actorBB.E.x)/2.0f + (speedNorm.x * distance);
ptz = (actorBB.P.z + actorBB.E.z)/2.0f + (speedNorm.z * distance);
}
if(it->Square.Overlap(ptx, ptz))
{
ModifiedSpeedY = (lastcheckedLayer - startY);
ModifiedSpeedY += (ModifiedSpeedY > 0) ? -0.0001f : 0.0001f;
water = it->IsWater;
return true;
}
}
}
return false;
}
