tf::Matrix3x3 JPCT_Math::quatToMatrix(tf::Quaternion q) {
tf::Matrix3x3 matrix;
float norm = magnitudeSquared(q);
float s = (double)norm > 0.0?2.0f / norm:0.0F;
float xs = q.x() * s;
float ys = q.y() * s;
float zs = q.z() * s;
float xx = q.x() * xs;
float xy = q.x() * ys;
float xz = q.x() * zs;
float xw = q.w() * xs;
float yy = q.y() * ys;
float yz = q.y() * zs;
float yw = q.w() * ys;
float zz = q.z() * zs;
float zw = q.w() * zs;
matrix[0][0] = 1.0F - (yy + zz);
matrix[1][0] = xy - zw;
matrix[2][0] = xz + yw;
matrix[0][1] = xy + zw;
matrix[1][1] = 1.0F - (xx + zz);
matrix[2][1] = yz - xw;
matrix[0][2] = xz - yw;
matrix[1][2] = yz + xw;
matrix[2][2] = 1.0F - (xx + yy);
return matrix;
}
void Collider::collide()
{
const int size = ents_.size();
for (int i = 0; i < size; ++i)
{
for (int j = i + 1; j < size; ++j)
{
CEntity* e1 = ents_[i];
CEntity* e2 = ents_[j];
CollisionCircle& c1 = e1->getCollisionCircle();
CollisionCircle& c2 = e2->getCollisionCircle();
float rad1 = e1->getRotation();
float rad2 = e2->getRotation();
tank::Vectorf pos1 = (c1.origin * -1.).rotate(rad1);
tank::Vectorf pos2 = (c2.origin * -1.).rotate(rad2);
pos1 += c1.pos;
pos2 += c2.pos;
const auto distance = pos2 - pos1;
const auto radSum = c1.radius + c2.radius;
if(distance.magnitudeSquared() < radSum*radSum)
{
c1.colliding = c2.colliding = true;
e1->collide(e2);
e2->collide(e1);
}
}
}
}
CollisionData collideWithCircle(
PointMovement const& p,
Circle const& c,
float time) {
/* Let c be the centre of the circle, and let r be its radius. Then then
* equation for the circle is
* |x - c|^2 = r^2
*
* Let v be the velocity of the point, and let p be the position of the
* point. Then the position of the point after time t is
* x = p + v t.
*
* Substituting that back in, the equation to be solved is:
* |v t + p - c|^2 = r^2.
* I.e.
* A t^2 + B t + C = 0,
* where
* A = |v|^2,
* B = 2 v . (p - c), and
* C = |p - c|^2 - r^2.
*
* We are looking for the least positive solution to this where t < time.
* If there is only one positive solution we may ingore it as this
* corresponds to the case when the point is inside the ball.
*/
float A = magnitudeSquared(p.vel);
float B = 2 * (dot(p.vel, p.pos - c.centre));
float C = magnitudeSquared(p.pos - c.centre) - c.radius * c.radius;
/* If velocity is 0 don't collide with this algorithm
* If there is at most one positive solution we don't collide.
* If there are no solutions.
*/
if (A <= 0 or
B > 0 or
B * B - 4 * A * C < 0) {
return {{0,0}, time};
}
float t = (-B - std::sqrt(B * B - 4 * A * C)) / (2 * A);
return { unit(c.centre - (p.pos + p.vel * t)), std::min(time, t) };
}
//---------------------------------------
const Vector2& Vector2::normalise()
{
if( x != 0.0 || y != 0.0 )
{
Real magSquared = magnitudeSquared();
if( magSquared != 1.0 )
{
*this *= 1.0 / fsm::sqrt(magSquared);
}
}
return *this;
}
int main() {
auto a = 1;
auto& b = a;
b = 2;
std::cout << a << std::endl;
auto u = poly::Vector2d{2,3,4};
auto v = poly::Vector2d::buildWithAngleAndMagnitude(M_PI, 1);
auto w = 3 * (u + v) * 8;
std::cout << w.magnitudeSquared() << std::endl;
auto circle = poly::Circle(poly::Vector2d{100,100}, 75);
auto rect = poly::Rectangle(poly::Vector2d{50,50}, poly::Vector2d{100,100});
return 0;
}
data_type magnitude() const {
return sqrt(magnitudeSquared());
}
inline_ float magnitude() const
{
return sqrtf(magnitudeSquared());
}
int main( int argc, char* argv[] )
{
if ( argc != 2 ) {
showUsage();
return 0;
}
// Start the rng.
RandomGenerator rng( 41u );
UniformRandomSize sizeTypeRng( rng, boost::uniform_int< std::size_t >( 0,
std::numeric_limits< std::size_t >::max() ) );
// Parse the input file.
std::ifstream input( argv[1] );
std::string line;
// The interaction database file.
std::string xmlDatabaseFile;
input >> xmlDatabaseFile; getline( input, line );
InteractionDatabase myDatabase( xmlDatabaseFile );
// Time and time step.
double deltaTime, endTime, outputDeltaTime;
input >> deltaTime; getline( input, line );
input >> endTime; getline( input, line );
// Output info.
input >> outputDeltaTime; getline( input, line );
std::size_t numberOfOutputBins;
input >> numberOfOutputBins; getline( input, line );
// Volume of the domain.
double cellVolumeCubicMeters;
input >> cellVolumeCubicMeters; getline( input, line );
// Particle species, counts, and temperatures.
std::multimap< std::string, ParticleInputSpec > particleInitialConditions;
while ( true ) {
std::string speciesName;
std::size_t particleCount;
double particleTemperature;
Vec3 commonVelocity;
input >> speciesName >> particleCount >> particleTemperature >> commonVelocity;
if ( ! input.good() ) {
break;
} else {
getline( input, line );
}
ParticleInputSpec inputSpec = { particleCount, particleTemperature, commonVelocity };
particleInitialConditions.insert( std::make_pair( speciesName, inputSpec ) );
}
input.close();
// Add all species types to the database.
typedef std::multimap< std::string, ParticleInputSpec >::const_iterator ConstSpecIterator;
for ( ConstSpecIterator speciesIterator = particleInitialConditions.begin();
speciesIterator != particleInitialConditions.end();
speciesIterator = particleInitialConditions.upper_bound( speciesIterator->first ) ) {
myDatabase.addParticleType( speciesIterator->first );
}
// Throw away all interactions except elastic reactions.
// myDatabase.filter = boost::make_shared< chimp::interaction::filter::Elastic >();
// Finalize the database and do housekeeping.
myDatabase.initBinaryInteractions();
myDatabase.xmlDb.close();
// Now make up some particles. We use the same indexing as the particle database to ease bookkeeping.
typedef std::vector< Particle > AllParticlesOfOneSpecies;
std::vector< AllParticlesOfOneSpecies > speciesSets( myDatabase.getProps().size() );
for ( int i = 0; i < myDatabase.getProps().size(); ++i ) {
// Find the pertinent particle specs.
std::string speciesName = myDatabase[i].name::value;
for ( ConstSpecIterator speciesIterator = particleInitialConditions.lower_bound( speciesName );
speciesIterator != particleInitialConditions.upper_bound( speciesName ); ++speciesIterator ) {
const ParticleInputSpec& spec = speciesIterator->second;
// Give the particles some random velocities plus the common velocity.
for ( std::size_t particleIndex = 0; particleIndex < spec.count; ++particleIndex ) {
Particle particle;
particle.velocity =
randomVelocityFromMaxwellian( rng, spec.temperatureInKelvin, myDatabase[i].mass::value );
particle.velocity += spec.driftVelocity;
speciesSets[i].push_back( particle );
}
}
}
// Write some info for the reaction set.
std::cout << "BEGIN INTERACTION TABLE" << std::endl;
BOOST_FOREACH( const InteractionDatabase::Set& reactionBranches, myDatabase.getInteractions() ) {
if ( reactionBranches.rhs.size() == 0 ) continue;
std::cout << " Reactions of "
<< myDatabase[ reactionBranches.lhs.A.species ].name::value << " with "
<< myDatabase[ reactionBranches.lhs.B.species ].name::value << std::endl;
BOOST_FOREACH( const InteractionDatabase::Set::Equation& eq, reactionBranches.rhs ) {
eq.print( std::cout << " ", myDatabase ) << std::endl;
}
}
std::cout << "END INTERACTION TABLE" << std::endl;
// Loop over time steps.
const double beginTime = 0.;
for ( double time = beginTime; time < endTime; time += deltaTime ) {
// Update particle positions, max speeds, and temperatures.
std::vector< double > maxSpeedMetersPerSecond;
std::vector< double > speciesTemperatures;
std::size_t collisionCount = 0;
std::size_t particleCount = 0;
for ( int speciesIndex = 0; speciesIndex < speciesSets.size(); ++speciesIndex ) {
maxSpeedMetersPerSecond.push_back( 0. );
speciesTemperatures.push_back( 0. );
BOOST_FOREACH( Particle& particle, speciesSets[ speciesIndex ] ) {
// v = v + E * dt;
// x = x + v * dt;
double particleSpeedSquared = magnitudeSquared( particle.velocity );
maxSpeedMetersPerSecond.back() =
std::max( maxSpeedMetersPerSecond.back(), sqrt( particleSpeedSquared ) );
speciesTemperatures.back() += particleSpeedSquared;
++particleCount;
}
speciesTemperatures.back() *= 0.5 * myDatabase[ speciesIndex ].mass::value; // kinetic energy
speciesTemperatures.back() /= 1.5 * physical::constant::si::K_B * speciesSets[ speciesIndex ].size();
}
if ( time == beginTime ) {
printConsoleData( myDatabase, speciesTemperatures, 0.0, 0, particleCount, true );
}
// Collide a few particles. This is done by looping over all reactions, choosing a few pairs of
// the pertinent 2 species, and doing a null-collision algorithm on these pairs.
BOOST_FOREACH( const InteractionDatabase::Set& reactionBranches, myDatabase.getInteractions() ) {
if ( reactionBranches.rhs.size() == 0 ) continue;
std::vector< std::size_t > particlesPerSpecies;
std::vector< int > reactantIndices;
reactantIndices.push_back( reactionBranches.lhs.A.species );
reactantIndices.push_back( reactionBranches.lhs.B.species );
std::size_t numberOfCollisions = 1;
BOOST_FOREACH( const int& reactantIndex, reactantIndices ) {
particlesPerSpecies.push_back( speciesSets[ reactantIndex ].size() );
numberOfCollisions *= particlesPerSpecies.back();
}
// Round the number of collisions based on the remainder using MC method.
double maxRelativeSpeed = std::accumulate( maxSpeedMetersPerSecond.begin(),
maxSpeedMetersPerSecond.end(), 0. );
double maxSigmaSpeedProduct = reactionBranches.findMaxSigmaVProduct( maxRelativeSpeed );
double fractionalCollisions =
numberOfCollisions * maxSigmaSpeedProduct * deltaTime / cellVolumeCubicMeters;
numberOfCollisions = static_cast< std::size_t >( std::floor( fractionalCollisions ) );
double partialCollision = fractionalCollisions - numberOfCollisions;
UniformRandomDouble remainderSelector( rng, boost::uniform_real<>( 0., 1. ) );
if ( remainderSelector() < partialCollision ) {
++numberOfCollisions;
}
// Setup some dummy vectors to hold arguments.
std::vector< InteractionDatabase::options::Particle > reactants( 2 );
std::vector< const InteractionDatabase::options::Particle* > reactantPointers( 2 );
for ( int i = 0; i < 2; ++i ) {
reactantPointers[i] = &reactants[i];
}
// Loop over all of the chosen collision pairs.
for ( std::size_t collisionPair = 0; collisionPair < numberOfCollisions; ++collisionPair ) {
// Choose the particles randomly, making sure we don't pick the same particle twice if this
// is a like-pair collision.
boost::array< std::size_t, 2 > particleIndex;
particleIndex[0] = sizeTypeRng() % particlesPerSpecies[0];
particleIndex[1] = sizeTypeRng() % particlesPerSpecies[1];
if ( reactantIndices[0] == reactantIndices[1] ) {
while ( particleIndex[1] == particleIndex[0] ) {
particleIndex[1] = sizeTypeRng() % particlesPerSpecies[1];
}
}
// Copy over the velocities.
for ( int i = 0; i < 2; ++i ) {
for ( int j = 0; j < 3; ++j )
reactants[i].v[j] =
speciesSets[ reactantIndices[i] ][ particleIndex[i] ].velocity[j];
}
double relativeSpeed = magnitude( reactants[0].v - reactants[1].v );
if ( relativeSpeed == 0.0 ) {
continue;
}
// Find which branch to take using null collision method.
std::pair< int, double > path =
reactionBranches.calculateOutPath( maxSigmaSpeedProduct, relativeSpeed );
// Skip it if there is no collision.
if ( path.first < 0 ) {
continue;
}
const InteractionDatabase::Set::Equation& selectedPath = reactionBranches.rhs[ path.first ];
InteractionDatabase::Interaction::ParticleParam defaultParam;
defaultParam.is_set = false; // because it doesn't have a default constructor yet.
std::vector< InteractionDatabase::Interaction::ParticleParam > products( 5, defaultParam );
selectedPath.interaction->interact( reactantPointers, products );
++collisionCount;
// For elastic collisions, we just copy the velocities of the products.
for ( int i = 0; i < 2; ++i ) {
std::vector< Particle >& species = speciesSets[ reactantIndices[i] ];
if ( products[i].is_set ) {
// The original reactant was modified, but was not consumed.
for ( int j = 0; j < 3; ++j ) {
species[ particleIndex[i] ].velocity[j] = products[i].particle.v[j];
}
} else {
// The reactant was consumed. Remove it.
species.erase( species.begin() + particleIndex[i] );
}
}
// Add the new particles if there were any created.
for ( int i = 2; i < 5; ++i ) {
if ( products[i].is_set ) {
Particle newParticle;
for ( int j = 0; j < 3; ++j ) {
newParticle.velocity[j] = products[i].particle.v[j];
}
speciesSets[ selectedPath.products[i-2].species ].push_back( newParticle );
}
}
} // collisionPair
} // interaction
auto project(Point<PRECISION> source, Point<PRECISION2> wall) {
return multiply(wall, dotProduct(wall, source) / magnitudeSquared(wall));
}
auto magnitude(Point<PRECISION> p) {
return sqrt(magnitudeSquared(p));
}
long double magnitude() const
{
return sqrt(magnitudeSquared());
}
const T magnitude() const { return sqrt(magnitudeSquared()); }
double FeatureVector::magnitude() const {
return sqrt(magnitudeSquared());
}
T magnitude(sf::Vector2<T> const& v) {
return std::sqrt(magnitudeSquared(v));
}
