unsigned CollisionDetector::boxAndSphere(
const CollisionBox &box,
const CollisionSphere &sphere,
CollisionData *data
)
{
// Transform the centre of the sphere into box coordinates
Vector3 centre = sphere.getAxis(3);
Vector3 relCentre = box.transform.transformInverse(centre);
// Early out check to see if we can exclude the contact
if (real_abs(relCentre.x) - sphere.radius > box.halfSize.x ||
real_abs(relCentre.y) - sphere.radius > box.halfSize.y ||
real_abs(relCentre.z) - sphere.radius > box.halfSize.z)
{
return 0;
}
Vector3 closestPt(0,0,0);
real dist;
// Clamp each coordinate to the box.
dist = relCentre.x;
if (dist > box.halfSize.x) dist = box.halfSize.x;
if (dist < -box.halfSize.x) dist = -box.halfSize.x;
closestPt.x = dist;
dist = relCentre.y;
if (dist > box.halfSize.y) dist = box.halfSize.y;
if (dist < -box.halfSize.y) dist = -box.halfSize.y;
closestPt.y = dist;
dist = relCentre.z;
if (dist > box.halfSize.z) dist = box.halfSize.z;
if (dist < -box.halfSize.z) dist = -box.halfSize.z;
closestPt.z = dist;
// Check we're in contact
dist = (closestPt - relCentre).squareMagnitude();
if (dist > sphere.radius * sphere.radius) return 0;
// Compile the contact
Vector3 closestPtWorld = box.transform.transform(closestPt);
Contact* contact = data->contacts;
contact->contactNormal = (closestPtWorld - centre);
contact->contactNormal.normalise();
contact->contactPoint = closestPtWorld;
contact->penetration = sphere.radius - real_sqrt(dist);
contact->setBodyData(box.body, sphere.body,
data->friction, data->restitution);
data->addContacts(1);
return 1;
}
int main() {
// Tuples are just a ordered set of values of (possibly) different types
std::tuple<bool, double> tuple {true, 2.0};
// To get an element of a tuple just use std::get<n>
// tuples are indexed from zero
// unlike structs, they are not required to be stored in order
std::cout << std::get<1>(tuple) << std::endl; // "2"
std::cout << sqrt(-1) << std::endl; // "-nan"
// One use of tuples is to return multiple values from functions
// without using "output parameters" as in C#
// note the "&&", that means the variable result is reference to a rvalue
std::tuple<bool, double>&& result = real_sqrt(-2.0);
if (std::get<0>(result))
std::cout << std::get<1>(result) << std::endl;
else
std::cout << "negative value fed" << std::endl;
// The standard library provides the function "tie" that
// conveniently assigns a tuple to different variables
bool sqrt_succeded;
double sqrt_result;
std::tie(sqrt_succeded, sqrt_result) = real_sqrt(2.0);
// Some values on a tuple can be ignored with the tie function
std::tie(std::ignore, sqrt_result) = real_sqrt(2.0);
// the function "tuple_cat" concatenates two or more tuples into one
std::tuple_cat(std::make_tuple(1,2), std::make_tuple(1,2,3));
// We define a function result type that generalizes that notion of
// functions that "fail"
function_result<double>&& result2 = real_sqrt2(-2);
if (std::get<0>(result2))
std::cout << std::get<1>(result2) << std::endl;
else
std::cout << "negative value fed" << std::endl;
}
void Quaternion::Normalize()
{
real d = (m_R*m_R) + (m_I*m_I) + (m_J*m_J) + (m_K*m_K);
//Zero length quaternion, so give no-rotation quaternion.
if(d==0)
{
m_R = 1;
return;
}
d = ((real)1.0/real_sqrt(d));
m_R *= d;
m_I *= d;
m_J *= d;
m_K *= d;
}
void game_physics_engine::CParticalFakeSpring::UpdateForce( CPartical* pPartical, const real duration )
{
if (!pPartical->HasFiniteMass())
{
return;
}
CVector3 position = pPartical->GetPosition();
position -= *m_pAnchor;
real gamma = 0.5f * (real_sqrt(4 * m_fSpringConstant - m_fDamping * m_fDamping));
CVector3 c = position * (m_fDamping / (2 * gamma)) + pPartical->GetVelocity() * (1.0f / gamma);
CVector3 target = position * real_cos(gamma * duration) + c * real_sin(gamma * duration);
target *= real_exp(-0.5f * duration * m_fDamping);
CVector3 accel = (target - position) * (1.0f / duration * duration) - pPartical->GetVelocity() * duration;
pPartical->AddForce(accel * pPartical->GetMass());
}
real Vector3::Magnitude() const
{
return real_sqrt(m_X*m_X + m_Y*m_Y + m_Z*m_Z);
}
static real_t inv_sqrt(real_t x)
{
return REAL(1.0) / real_sqrt(x);
}
unsigned Platform::addContact(cyclone::ParticleContact *contact,
unsigned limit) const
{
const static cyclone::real restitution = 0.0f;
unsigned used = 0;
for (unsigned i = 0; i < BLOB_COUNT; i++)
{
if (used >= limit) break;
// Check for penetration
cyclone::Vector3 toParticle = particles[i].getPosition() - start;
cyclone::Vector3 lineDirection = end - start;
cyclone::real projected = toParticle * lineDirection;
cyclone::real platformSqLength = lineDirection.squareMagnitude();
if (projected <= 0)
{
// The blob is nearest to the start point
if (toParticle.squareMagnitude() < BLOB_RADIUS*BLOB_RADIUS)
{
// We have a collision
contact->contactNormal = toParticle.unit();
contact->contactNormal.z = 0;
contact->restitution = restitution;
contact->particle[0] = particles + i;
contact->particle[1] = 0;
contact->penetration = BLOB_RADIUS - toParticle.magnitude();
used ++;
contact ++;
}
}
else if (projected >= platformSqLength)
{
// The blob is nearest to the end point
toParticle = particles[i].getPosition() - end;
if (toParticle.squareMagnitude() < BLOB_RADIUS*BLOB_RADIUS)
{
// We have a collision
contact->contactNormal = toParticle.unit();
contact->contactNormal.z = 0;
contact->restitution = restitution;
contact->particle[0] = particles + i;
contact->particle[1] = 0;
contact->penetration = BLOB_RADIUS - toParticle.magnitude();
used ++;
contact ++;
}
}
else
{
// the blob is nearest to the middle.
cyclone::real distanceToPlatform =
toParticle.squareMagnitude() -
projected*projected / platformSqLength;
if (distanceToPlatform < BLOB_RADIUS*BLOB_RADIUS)
{
// We have a collision
cyclone::Vector3 closestPoint =
start + lineDirection*(projected/platformSqLength);
contact->contactNormal = (particles[i].getPosition()-closestPoint).unit();
contact->contactNormal.z = 0;
contact->restitution = restitution;
contact->particle[0] = particles + i;
contact->particle[1] = 0;
contact->penetration = BLOB_RADIUS - real_sqrt(distanceToPlatform);
used ++;
contact ++;
}
}
}
return used;
}