void MobileVRInterface::set_position_from_sensors() {
_THREAD_SAFE_METHOD_
// this is a helper function that attempts to adjust our transform using our 9dof sensors
// 9dof is a misleading marketing term coming from 3 accelerometer axis + 3 gyro axis + 3 magnetometer axis = 9 axis
// but in reality this only offers 3 dof (yaw, pitch, roll) orientation
uint64_t ticks = OS::get_singleton()->get_ticks_usec();
uint64_t ticks_elapsed = ticks - last_ticks;
float delta_time = (double)ticks_elapsed / 1000000.0;
// few things we need
Input *input = Input::get_singleton();
Vector3 down(0.0, -1.0, 0.0); // Down is Y negative
Vector3 north(0.0, 0.0, 1.0); // North is Z positive
// make copies of our inputs
bool has_grav = false;
Vector3 acc = input->get_accelerometer();
Vector3 gyro = input->get_gyroscope();
Vector3 grav = input->get_gravity();
Vector3 magneto = scale_magneto(input->get_magnetometer()); // this may be overkill on iOS because we're already getting a calibrated magnetometer reading
if (sensor_first) {
sensor_first = false;
} else {
acc = scrub(acc, last_accerometer_data, 2, 0.2);
magneto = scrub(magneto, last_magnetometer_data, 3, 0.3);
};
last_accerometer_data = acc;
last_magnetometer_data = magneto;
if (grav.length() < 0.1) {
// not ideal but use our accelerometer, this will contain shakey shakey user behaviour
// maybe look into some math but I'm guessing that if this isn't available, its because we lack the gyro sensor to actually work out
// what a stable gravity vector is
grav = acc;
if (grav.length() > 0.1) {
has_grav = true;
};
} else {
has_grav = true;
};
bool has_magneto = magneto.length() > 0.1;
if (gyro.length() > 0.1) {
/* this can return to 0.0 if the user doesn't move the phone, so once on, it's on */
has_gyro = true;
};
if (has_gyro) {
// start with applying our gyro (do NOT smooth our gyro!)
Basis rotate;
rotate.rotate(orientation.get_axis(0), gyro.x * delta_time);
rotate.rotate(orientation.get_axis(1), gyro.y * delta_time);
rotate.rotate(orientation.get_axis(2), gyro.z * delta_time);
orientation = rotate * orientation;
tracking_state = ARVRInterface::ARVR_NORMAL_TRACKING;
};
///@TODO improve this, the magnetometer is very fidgity sometimes flipping the axis for no apparent reason (probably a bug on my part)
// if you have a gyro + accelerometer that combo tends to be better then combining all three but without a gyro you need the magnetometer..
if (has_magneto && has_grav && !has_gyro) {
// convert to quaternions, easier to smooth those out
Quat transform_quat(orientation);
Quat acc_mag_quat(combine_acc_mag(grav, magneto));
transform_quat = transform_quat.slerp(acc_mag_quat, 0.1);
orientation = Basis(transform_quat);
tracking_state = ARVRInterface::ARVR_NORMAL_TRACKING;
} else if (has_grav) {
// use gravity vector to make sure down is down...
// transform gravity into our world space
grav.normalize();
Vector3 grav_adj = orientation.xform(grav);
float dot = grav_adj.dot(down);
if ((dot > -1.0) && (dot < 1.0)) {
// axis around which we have this rotation
Vector3 axis = grav_adj.cross(down);
axis.normalize();
Basis drift_compensation(axis, acos(dot) * delta_time * 10);
orientation = drift_compensation * orientation;
};
};
// JIC
orientation.orthonormalize();
last_ticks = ticks;
};
Vector3 operator*(const Matrix4 &lhs, const Vector3 &rhs)
{
return Vector3(rhs.Transform(lhs));
}
bool Cube::setLimits(const Molecule &mol, double spacing_, double padding)
{
Index numAtoms = mol.atomCount();
Vector3 min_, max_;
if (numAtoms) {
Vector3 curPos = min_ = max_ = mol.atomPositions3d()[0];
for (Index i = 1; i < numAtoms; ++i) {
curPos = mol.atomPositions3d()[i];
if (curPos.x() < min_.x())
min_.x() = curPos.x();
if (curPos.x() > max_.x())
max_.x() = curPos.x();
if (curPos.y() < min_.y())
min_.y() = curPos.y();
if (curPos.y() > max_.y())
max_.y() = curPos.y();
if (curPos.z() < min_.z())
min_.z() = curPos.z();
if (curPos.z() > max_.z())
max_.z() = curPos.z();
}
}
else {
min_ = max_ = Vector3::Zero();
}
// Now to take care of the padding term
min_ += Vector3(-padding,-padding,-padding);
max_ += Vector3( padding, padding, padding);
return setLimits(min_, max_, spacing_);
}
float Vector3::angle(const Vector3& v) const
{
float angle = dot(v)/(magnitude()*v.magnitude());
return acos(angle);
}
bool Wml::FindIntersection (const Sphere3<Real>& rkS0,
const Sphere3<Real>& rkS1, Real fTime, const Vector3<Real>& rkV0,
const Vector3<Real>& rkV1, Real& rfFirstTime,
Vector3<Real>& rkFirstPoint)
{
Vector3<Real> kVDiff = rkV1 - rkV0;
Real fA = kVDiff.SquaredLength();
Vector3<Real> kCDiff = rkS1.Center() - rkS0.Center();
Real fC = kCDiff.SquaredLength();
Real fRSum = rkS0.Radius() + rkS1.Radius();
Real fRSumSqr = fRSum*fRSum;
if ( fA > (Real)0.0 )
{
Real fB = kCDiff.Dot(kVDiff);
if ( fB <= (Real)0.0 )
{
if ( -fTime*fA <= fB
|| fTime*(fTime*fA + ((Real)2.0)*fB) + fC <= fRSumSqr )
{
Real fCDiff = fC - fRSumSqr;
Real fDiscr = fB*fB - fA*fCDiff;
if ( fDiscr >= (Real)0.0 )
{
if ( fCDiff <= (Real)0.0 )
{
// The spheres are initially intersecting. Estimate a
// point of contact by using the midpoint of the line
// segment connecting the sphere centers.
rfFirstTime = (Real)0.0;
rkFirstPoint = ((Real)0.5)*(rkS0.Center() +
rkS1.Center());
}
else
{
// The first time of contact is in [0,fTime].
rfFirstTime = -(fB + Math<Real>::Sqrt(fDiscr))/fA;
if ( rfFirstTime < (Real)0.0 )
rfFirstTime = (Real)0.0;
else if ( rfFirstTime > fTime )
rfFirstTime = fTime;
Vector3<Real> kNewCDiff = kCDiff + rfFirstTime*kVDiff;
rkFirstPoint = rkS0.Center() + rfFirstTime*rkV0 +
(rkS0.Radius()/fRSum)*kNewCDiff;
}
return true;
}
}
return false;
}
}
if ( fC <= fRSumSqr )
{
// The spheres are initially intersecting. Estimate a point of
// contact by using the midpoint of the line segment connecting the
// sphere centers.
rfFirstTime = (Real)0.0;
rkFirstPoint = ((Real)0.5)*(rkS0.Center() + rkS1.Center());
return true;
}
return false;
}
//----------------------------------------------------------------------------
Vector3 Curve3::GetTangent (Real fTime) const
{
Vector3 kVelocity = GetFirstDerivative(fTime);
kVelocity.Unitize();
return kVelocity;
}
bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result) {
//give me back regular physics engine logic
//this is madness
//and most people using this function will think
//what it does is simpler than using physics
//this took about a week to get right..
//but is it right? who knows at this point..
if (r_result) {
r_result->collider_id = 0;
r_result->collider_shape = 0;
}
AABB body_aabb;
for (int i = 0; i < p_body->get_shape_count(); i++) {
if (i == 0)
body_aabb = p_body->get_shape_aabb(i);
else
body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
}
// Undo the currently transform the physics server is aware of and apply the provided one
body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
body_aabb = body_aabb.grow(p_margin);
Transform body_transform = p_from;
{
//STEP 1, FREE BODY IF STUCK
const int max_results = 32;
int recover_attempts = 4;
Vector3 sr[max_results * 2];
do {
PhysicsServerSW::CollCbkData cbk;
cbk.max = max_results;
cbk.amount = 0;
cbk.ptr = sr;
PhysicsServerSW::CollCbkData *cbkptr = &cbk;
CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk;
bool collided = false;
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int j = 0; j < p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_disabled(j))
continue;
Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);
ShapeSW *body_shape = p_body->get_shape(j);
for (int i = 0; i < amount; i++) {
const CollisionObjectSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) {
collided = cbk.amount > 0;
}
}
}
if (!collided) {
break;
}
Vector3 recover_motion;
for (int i = 0; i < cbk.amount; i++) {
Vector3 a = sr[i * 2 + 0];
Vector3 b = sr[i * 2 + 1];
recover_motion += (b - a) * 0.4;
}
if (recover_motion == Vector3()) {
collided = false;
break;
}
body_transform.origin += recover_motion;
body_aabb.position += recover_motion;
recover_attempts--;
} while (recover_attempts);
}
real_t safe = 1.0;
real_t unsafe = 1.0;
int best_shape = -1;
{
// STEP 2 ATTEMPT MOTION
AABB motion_aabb = body_aabb;
motion_aabb.position += p_motion;
motion_aabb = motion_aabb.merge(body_aabb);
int amount = _cull_aabb_for_body(p_body, motion_aabb);
for (int j = 0; j < p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_disabled(j))
continue;
Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);
ShapeSW *body_shape = p_body->get_shape(j);
Transform body_shape_xform_inv = body_shape_xform.affine_inverse();
MotionShapeSW mshape;
mshape.shape = body_shape;
mshape.motion = body_shape_xform_inv.basis.xform(p_motion);
bool stuck = false;
real_t best_safe = 1;
real_t best_unsafe = 1;
for (int i = 0; i < amount; i++) {
const CollisionObjectSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
//test initial overlap, does it collide if going all the way?
Vector3 point_A, point_B;
Vector3 sep_axis = p_motion.normalized();
Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
if (CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
//print_line("failed motion cast (no collision)");
continue;
}
sep_axis = p_motion.normalized();
if (!CollisionSolverSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
//print_line("failed motion cast (no collision)");
stuck = true;
break;
}
//just do kinematic solving
real_t low = 0;
real_t hi = 1;
Vector3 mnormal = p_motion.normalized();
for (int i = 0; i < 8; i++) { //steps should be customizable..
real_t ofs = (low + hi) * 0.5;
Vector3 sep = mnormal; //important optimization for this to work fast enough
mshape.motion = body_shape_xform_inv.basis.xform(p_motion * ofs);
Vector3 lA, lB;
bool collided = !CollisionSolverSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep);
if (collided) {
//print_line(itos(i)+": "+rtos(ofs));
hi = ofs;
} else {
point_A = lA;
point_B = lB;
low = ofs;
}
}
if (low < best_safe) {
best_safe = low;
best_unsafe = hi;
}
}
if (stuck) {
safe = 0;
unsafe = 0;
best_shape = j; //sadly it's the best
break;
}
if (best_safe == 1.0) {
continue;
}
if (best_safe < safe) {
safe = best_safe;
unsafe = best_unsafe;
best_shape = j;
}
}
}
bool collided = false;
if (safe >= 1) {
//not collided
collided = false;
if (r_result) {
r_result->motion = p_motion;
r_result->remainder = Vector3();
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}
} else {
//it collided, let's get the rest info in unsafe advance
Transform ugt = body_transform;
ugt.origin += p_motion * unsafe;
_RestCallbackData rcd;
rcd.best_len = 0;
rcd.best_object = NULL;
rcd.best_shape = 0;
Transform body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
ShapeSW *body_shape = p_body->get_shape(best_shape);
body_aabb.position += p_motion * unsafe;
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int i = 0; i < amount; i++) {
const CollisionObjectSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
rcd.object = col_obj;
rcd.shape = shape_idx;
bool sc = CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, NULL, p_margin);
if (!sc)
continue;
}
if (rcd.best_len != 0) {
if (r_result) {
r_result->collider = rcd.best_object->get_self();
r_result->collider_id = rcd.best_object->get_instance_id();
r_result->collider_shape = rcd.best_shape;
r_result->collision_local_shape = best_shape;
r_result->collision_normal = rcd.best_normal;
r_result->collision_point = rcd.best_contact;
//r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
const BodySW *body = static_cast<const BodySW *>(rcd.best_object);
//Vector3 rel_vec = r_result->collision_point - body->get_transform().get_origin();
// r_result->collider_velocity = Vector3(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
r_result->collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - rcd.best_contact); // * mPos);
r_result->motion = safe * p_motion;
r_result->remainder = p_motion - safe * p_motion;
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}
collided = true;
} else {
if (r_result) {
r_result->motion = p_motion;
r_result->remainder = Vector3();
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}
collided = false;
}
}
return collided;
}
bool XMLElement::SetVector3(const String& name, const Vector3& value)
{
return SetAttribute(name, value.ToString());
}
bool ParticleEmitter::EmitNewParticle()
{
unsigned index = GetFreeParticle();
if (index == M_MAX_UNSIGNED)
return false;
assert(index < particles_.Size());
Particle& particle = particles_[index];
Billboard& billboard = billboards_[index];
Vector3 startDir;
Vector3 startPos;
startDir = effect_->GetRandomDirection();
startDir.Normalize();
switch (effect_->GetEmitterType())
{
case EMITTER_SPHERE:
{
Vector3 dir(
Random(2.0f) - 1.0f,
Random(2.0f) - 1.0f,
Random(2.0f) - 1.0f
);
dir.Normalize();
startPos = effect_->GetEmitterSize() * dir * 0.5f;
}
break;
case EMITTER_BOX:
{
const Vector3& emitterSize = effect_->GetEmitterSize();
startPos = Vector3(
Random(emitterSize.x_) - emitterSize.x_ * 0.5f,
Random(emitterSize.y_) - emitterSize.y_ * 0.5f,
Random(emitterSize.z_) - emitterSize.z_ * 0.5f
);
}
break;
}
particle.size_ = effect_->GetRandomSize();
particle.timer_ = 0.0f;
particle.timeToLive_ = effect_->GetRandomTimeToLive();
particle.scale_ = 1.0f;
particle.rotationSpeed_ = effect_->GetRandomRotationSpeed();
particle.colorIndex_ = 0;
particle.texIndex_ = 0;
if (faceCameraMode_ == FC_DIRECTION)
{
startPos += startDir * particle.size_.y_;
}
if (!relative_)
{
startPos = node_->GetWorldTransform() * startPos;
startDir = node_->GetWorldRotation() * startDir;
};
particle.velocity_ = effect_->GetRandomVelocity() * startDir;
billboard.position_ = startPos;
billboard.size_ = particles_[index].size_;
const Vector<TextureFrame>& textureFrames_ = effect_->GetTextureFrames();
billboard.uv_ = textureFrames_.Size() ? textureFrames_[0].uv_ : Rect::POSITIVE;
billboard.rotation_ = effect_->GetRandomRotation();
const Vector<ColorFrame>& colorFrames_ = effect_->GetColorFrames();
billboard.color_ = colorFrames_.Size() ? colorFrames_[0].color_ : Color();
billboard.enabled_ = true;
billboard.direction_ = startDir;
return true;
}
//-----------------------------------------------------------------------
void ConvexBody::clip( const Plane& pl, bool keepNegative )
{
if ( getPolygonCount() == 0 )
return;
// current will be used as the reference body
ConvexBody current;
current.moveDataFromBody(*this);
OgreAssert( this->getPolygonCount() == 0, "Body not empty!" );
OgreAssert( current.getPolygonCount() != 0, "Body empty!" );
// holds all intersection edges for the different polygons
Polygon::EdgeMap intersectionEdges;
// clip all polygons by the intersection plane
// add only valid or intersected polygons to *this
for ( size_t iPoly = 0; iPoly < current.getPolygonCount(); ++iPoly )
{
// fetch vertex count and ignore polygons with less than three vertices
// the polygon is not valid and won't be added
const size_t vertexCount = current.getVertexCount( iPoly );
if ( vertexCount < 3 )
continue;
// current polygon
const Polygon& p = current.getPolygon( iPoly );
// the polygon to assemble
Polygon *pNew = allocatePolygon();
// the intersection polygon (indeed it's an edge or it's empty)
Polygon *pIntersect = allocatePolygon();
// check if polygons lie inside or outside (or on the plane)
// for each vertex check where it is situated in regard to the plane
// three possibilities appear:
Plane::Side clipSide = keepNegative ? Plane::POSITIVE_SIDE : Plane::NEGATIVE_SIDE;
// - side is clipSide: vertex will be clipped
// - side is !clipSide: vertex will be untouched
// - side is NOSIDE: vertex will be untouched
Plane::Side *side = OGRE_ALLOC_T(Plane::Side, vertexCount, MEMCATEGORY_SCENE_CONTROL);
for ( size_t iVertex = 0; iVertex < vertexCount; ++iVertex )
{
side[ iVertex ] = pl.getSide( p.getVertex( iVertex ) );
}
// now we check the side combinations for the current and the next vertex
// four different combinations exist:
// - both points inside (or on the plane): keep the second (add it to the body)
// - both points outside: discard both (don't add them to the body)
// - first vertex is inside, second is outside: add the intersection point
// - first vertex is outside, second is inside: add the intersection point, then the second
for ( size_t iVertex = 0; iVertex < vertexCount; ++iVertex )
{
// determine the next vertex
size_t iNextVertex = ( iVertex + 1 ) % vertexCount;
const Vector3& vCurrent = p.getVertex( iVertex );
const Vector3& vNext = p.getVertex( iNextVertex );
// case 1: both points inside (store next)
if ( side[ iVertex ] != clipSide && // NEGATIVE or NONE
side[ iNextVertex ] != clipSide ) // NEGATIVE or NONE
{
// keep the second
pNew->insertVertex( vNext );
}
// case 3: inside -> outside (store intersection)
else if ( side[ iVertex ] != clipSide &&
side[ iNextVertex ] == clipSide )
{
// Do an intersection with the plane. We use a ray with a start point and a direction.
// The ray is forced to hit the plane with any option available (eigher current or next
// is the starting point)
// intersect from the outside vertex towards the inside one
Vector3 vDirection = vCurrent - vNext;
vDirection.normalise();
Ray ray( vNext, vDirection );
std::pair< bool, Real > intersect = ray.intersects( pl );
// store intersection
if ( intersect.first )
{
// convert distance to vector
Vector3 vIntersect = ray.getPoint( intersect.second );
// store intersection
pNew->insertVertex( vIntersect );
pIntersect->insertVertex( vIntersect );
}
}
// case 4: outside -> inside (store intersection, store next)
else if ( side[ iVertex ] == clipSide &&
side[ iNextVertex ] != clipSide )
{
// Do an intersection with the plane. We use a ray with a start point and a direction.
// The ray is forced to hit the plane with any option available (eigher current or next
// is the starting point)
// intersect from the outside vertex towards the inside one
Vector3 vDirection = vNext - vCurrent;
vDirection.normalise();
Ray ray( vCurrent, vDirection );
std::pair< bool, Real > intersect = ray.intersects( pl );
// store intersection
if ( intersect.first )
{
// convert distance to vector
Vector3 vIntersect = ray.getPoint( intersect.second );
// store intersection
pNew->insertVertex( vIntersect );
pIntersect->insertVertex( vIntersect );
}
pNew->insertVertex( vNext );
}
// else:
// case 2: both outside (do nothing)
}
// insert the polygon only, if at least three vertices are present
if ( pNew->getVertexCount() >= 3 )
{
// in case there are double vertices, remove them
pNew->removeDuplicates();
// in case there are still at least three vertices, insert the polygon
if ( pNew->getVertexCount() >= 3 )
{
this->insertPolygon( pNew );
}
else
{
// delete pNew because it's empty or invalid
freePolygon(pNew);
pNew = 0;
}
}
else
{
// delete pNew because it's empty or invalid
freePolygon(pNew);
pNew = 0;
}
// insert intersection polygon only, if there are two vertices present
if ( pIntersect->getVertexCount() == 2 )
{
intersectionEdges.insert( Polygon::Edge( pIntersect->getVertex( 0 ),
pIntersect->getVertex( 1 ) ) );
}
// delete intersection polygon
// vertices were copied (if there were any)
freePolygon(pIntersect);
pIntersect = 0;
// delete side info
OGRE_FREE(side, MEMCATEGORY_SCENE_CONTROL);
side = 0;
}
// if the polygon was partially clipped, close it
// at least three edges are needed for a polygon
if ( intersectionEdges.size() >= 3 )
{
Polygon *pClosing = allocatePolygon();
// Analyze the intersection list and insert the intersection points in ccw order
// Each point is twice in the list because of the fact that we have a convex body
// with convex polygons. All we have to do is order the edges (an even-odd pair)
// in a ccw order. The plane normal shows us the direction.
Polygon::EdgeMap::iterator it = intersectionEdges.begin();
// check the cross product of the first two edges
Vector3 vFirst = it->first;
Vector3 vSecond = it->second;
// remove inserted edge
intersectionEdges.erase( it );
Vector3 vNext;
// find mating edge
if (findAndEraseEdgePair(vSecond, intersectionEdges, vNext))
{
// detect the orientation
// the polygon must have the same normal direction as the plane and then n
Vector3 vCross = ( vFirst - vSecond ).crossProduct( vNext - vSecond );
bool frontside = ( pl.normal ).directionEquals( vCross, Degree( 1 ) );
// first inserted vertex
Vector3 firstVertex;
// currently inserted vertex
Vector3 currentVertex;
// direction equals -> front side (walk ccw)
if ( frontside )
{
// start with next as first vertex, then second, then first and continue with first to walk ccw
pClosing->insertVertex( vNext );
pClosing->insertVertex( vSecond );
pClosing->insertVertex( vFirst );
firstVertex = vNext;
currentVertex = vFirst;
#ifdef _DEBUG_INTERSECTION_LIST
std::cout << "Plane: n=" << pl.normal << ", d=" << pl.d << std::endl;
std::cout << "First inserted vertex: " << *next << std::endl;
std::cout << "Second inserted vertex: " << *vSecond << std::endl;
std::cout << "Third inserted vertex: " << *vFirst << std::endl;
#endif
}
// direction does not equal -> back side (walk cw)
else
{
// start with first as first vertex, then second, then next and continue with next to walk ccw
pClosing->insertVertex( vFirst );
pClosing->insertVertex( vSecond );
pClosing->insertVertex( vNext );
firstVertex = vFirst;
currentVertex = vNext;
#ifdef _DEBUG_INTERSECTION_LIST
std::cout << "Plane: n=" << pl.normal << ", d=" << pl.d << std::endl;
std::cout << "First inserted vertex: " << *vFirst << std::endl;
std::cout << "Second inserted vertex: " << *vSecond << std::endl;
std::cout << "Third inserted vertex: " << *next << std::endl;
#endif
}
// search mating edges that have a point in common
// continue this operation as long as edges are present
while ( !intersectionEdges.empty() )
{
if (findAndEraseEdgePair(currentVertex, intersectionEdges, vNext))
{
// insert only if it's not the last (which equals the first) vertex
if ( !intersectionEdges.empty() )
{
currentVertex = vNext;
pClosing->insertVertex( vNext );
}
}
else
{
// degenerated...
break;
}
} // while intersectionEdges not empty
// insert polygon (may be degenerated!)
this->insertPolygon( pClosing );
}
// mating intersection edge NOT found!
else
{
freePolygon(pClosing);
}
} // if intersectionEdges contains more than three elements
}
//----------------------------------------------------------------------------------------------------
Vector3 Scene::getColor (Ray r, double intensity, int depth, double tolerance)
{
Vector3 ret;
Vector3 color;
Object *obj;
Vector3 normal;
Vector3 intersection;
int objIndex;
Vector3 diffuse, phong;
Ray lightR;
double d = getIntersection(&objIndex, r);
// Ray has no intersection
if (d <= 0.0)
return Vector3(0.0, 0.0, 0.0);
// Get intersection point
intersection = r.move(d);
obj = objects[objIndex];
color = obj->getColor();
normal = obj->getNormal(intersection);
// Ambient color
ret = 0.15 * color;
// For each light
int n = lights.size();
for (int i=0; i<n; i++)
{
lightR = Ray( intersection, lights[i].position - intersection );
// Verify if object is not in the shadow
if ( !hasIntersection(lightR, lights[i].position) )
{
Vector3 lightColor = lights[i].getColor();
// Calculate diffuse illumination
diffuse = color * max(normal * lightR.direction, 0.0);
diffuse = Vector3(lightColor.x * diffuse.x, lightColor.y * diffuse.y, lightColor.z * diffuse.z);
// Calculate specular illumination
Vector3 h = (lights[i].position - intersection) + (camera.position - intersection);
h.normalize();
phong += lightColor * pow(max(h * normal, 0.0), 128);
ret += diffuse;
}
}
Vector3 refColor;
double reflect = obj->reflect;
// Test base cases for recursion
if ((depth > 1) && (intensity*reflect > tolerance))
{
Ray refRay(intersection, r.direction - 2*(r.direction * normal) * normal);
refColor = getColor(refRay, reflect*intensity, --depth, tolerance);
}
// Sum all color and check for overflows
ret = intensity * ( (1 - reflect) * (0.9 * ret) + reflect * refColor + phong);
if (ret.x > 1)
ret.x = 1;
if (ret.y > 1)
ret.y = 1;
if (ret.z > 1)
ret.z = 1;
return ret;
}
//-----------------------------------------------------------------------
void ConvexBody::extend(const Vector3& pt)
{
// Erase all polygons facing towards the point. For all edges that
// are not removed twice (once in AB and once BA direction) build a
// convex polygon (triangle) with the point.
Polygon::EdgeMap edgeMap;
for ( size_t i = 0; i < getPolygonCount(); ++i )
{
const Vector3& normal = getNormal( i );
// direction of the point in regard to the polygon
// the polygon is planar so we can take an arbitrary vertex
Vector3 ptDir = pt - getVertex( i, 0 );
ptDir.normalise();
// remove polygon if dot product is greater or equals null.
if ( normal.dotProduct( ptDir ) >= 0 )
{
// store edges (copy them because if the polygon is deleted
// its vertices are also deleted)
storeEdgesOfPolygon( i, &edgeMap );
// remove polygon
deletePolygon( i );
// decrement iterator because of deleted polygon
--i;
}
}
// point is already a part of the hull (point lies inside)
if ( edgeMap.empty() )
return;
// remove the edges that are twice in the list (once from each side: AB,BA)
Polygon::EdgeMap::iterator it;
// iterate from first to the element before the last one
for (Polygon::EdgeMap::iterator itStart = edgeMap.begin();
itStart != edgeMap.end(); )
{
// compare with iterator + 1 to end
// don't need to skip last entry in itStart since omitted in inner loop
it = itStart;
++it;
bool erased = false;
// iterate from itStart+1 to the element before the last one
for ( ; it != edgeMap.end(); ++it )
{
if (itStart->first.positionEquals(it->second) &&
itStart->second.positionEquals(it->first))
{
edgeMap.erase(it);
// increment itStart before deletion (iterator invalidation)
Polygon::EdgeMap::iterator delistart = itStart++;
edgeMap.erase(delistart);
erased = true;
break; // found and erased
}
}
// increment itStart if we didn't do it when erasing
if (!erased)
++itStart;
}
// use the remaining edges to build triangles with the point
// the vertices of the edges are in ccw order (edgePtA-edgePtB-point
// to form a ccw polygon)
while ( !edgeMap.empty() )
{
Polygon::EdgeMap::iterator mapIt = edgeMap.begin();
// build polygon it.first, it.second, point
Polygon *p = allocatePolygon();
p->insertVertex(mapIt->first);
p->insertVertex(mapIt->second);
p->insertVertex( pt );
// attach polygon to body
insertPolygon( p );
// erase the vertices from the list
// pointers are now held by the polygon
edgeMap.erase( mapIt );
}
}
Vector3 operator*(const float &a, const Vector3 &vec) {
Vector3 ret(a*vec.x(), a*vec.y(), a*vec.z());
return ret;
}
void createTerrainLightmap(const TerrainInfo& info, Image& image,
size_t width, size_t height, Vector3 lightDir, const ColourValue& lightCol,
const ColourValue& ambient, bool shadowed)
{
lightDir.normalise();
// calculate lightmap by multiplying light dir with terrain normals
// calculate the step size to use
AxisAlignedBox extents = info.getExtents();
Vector3 startPos = extents.getMinimum();
Vector3 step = extents.getMaximum() - extents.getMinimum();
step.x /= width;
step.z /= height;
Vector3 pos = startPos;
#if OGRE_VERSION_MINOR > 4
// Ogre::Image uses the memory allocation macros internally in Shoggoth,
// so we must use them as well.
uchar* lightMap = OGRE_ALLOC_T(uchar, width*height*3, MEMCATEGORY_GENERAL);
#else
uchar* lightMap = new uchar[width*height * 3];
#endif
memset(lightMap, 255, width*height*3);
for (size_t z = 0; z < height; ++z)
{
for (size_t x = 0; x < width; ++x)
{
size_t index = (z * width + x)*3;
// calculate diffuse light from light source
Vector3 norm = info.getNormalAt(pos.x, pos.z);
float l = std::max(0.0f, -lightDir.dotProduct(norm));
ColourValue v = ambient;
v.r = std::min(1.0f, v.r+l*lightCol.r);
v.g = std::min(1.0f, v.g+l*lightCol.g);
v.b = std::min(1.0f, v.b+l*lightCol.b);
lightMap[index+0] = (uchar) (255*v.r);
lightMap[index+1] = (uchar) (255*v.g);
lightMap[index+2] = (uchar) (255*v.b);
pos.x += step.x;
}
pos.x = startPos.x;
pos.z += step.z;
}
if (shadowed && (lightDir.x != 0 || lightDir.z != 0))
{
// add terrain shadows
Impl::addTerrainShadowsToLightmap(lightMap, info, width, height, lightDir, ambient);
}
// use a box filter to smoothen the lightmap
Impl::boxFilterLightmap(lightMap, width, height);
// save lightmap to image
image.loadDynamicImage(lightMap, width, height, 1, PF_BYTE_RGB, true);
// ownership of lightMap was transfered to image, don't need to delete
}
void AILookStrategy::Step(unsigned timeMs)
{
IPhysical *phys = Parent->GetPhysical();
IScenable *scen = Parent->GetScenable();
if(RotationUnit.mRotating)
{
RotationUnit.Step();
if(RotationUnit.mRotating) // Process timed rotation
{
Ogre::Quaternion delta = RotationUnit.Slerp();
scen->SetOrientation(delta);
}
} else
{
Ogre::Quaternion OurOrientation = scen->GetOrientation();
//Ogre::Vector3 src = OurOrientation * Ogre::Vector3::NEGATIVE_UNIT_Z;
Ogre::Vector3 direction = Ogre::Vector3::ZERO;
switch(LookType) {
case LT_FORWARD:
direction = -phys->GetForwardDirection();
break;
case LT_DIRECTION:
direction = Direction;
break;
case LT_OBJECT:
{
if (TargetID<0)
{
assert(false);
}
if (TargetID==0)
{
if (Owner)
{
TargetID = Owner->SelectTargetID();
}
}
IAAObject *obj = CommonDeclarations::GetIDObject(TargetID);
//assert(obj);
if (obj)
direction = obj->GetPosition()-scen->GetPosition();
break;
}
};
if (!direction.isZeroLength())
{
Vector3 xVec = Ogre::Vector3::UNIT_Y.crossProduct(direction);
xVec.normalise();
Vector3 yVec = direction.crossProduct(xVec);
yVec.normalise();
Quaternion unitZToTarget = Quaternion(xVec, yVec, direction);
Quaternion targetOrientation = Quaternion(-unitZToTarget.y, -unitZToTarget.z, unitZToTarget.w, unitZToTarget.x);
RotationUnit.StartRotation(OurOrientation, targetOrientation);
}
}
}
double SteeringBehaviour :: getIntersectionTime (const Vector3& agent_position,
double agent_speed,
const Vector3& target_position,
const Vector3& target_velocity)
{
assert(agent_speed >= 0.0);
if(agent_speed == 0.0)
{
if(DEBUGGING_INTERSECTION_TIME)
cout << "No #1" << endl;
return NO_INTERSECTION_POSSIBLE; // can't catch if can't move
}
if(target_velocity.isZero())
{
if(DEBUGGING_INTERSECTION_TIME)
cout << "No #2" << endl;
return getIntersectionTime(agent_position, agent_speed, target_position);
}
Vector3 relative_position = target_position - agent_position;
double agent_speed_squared = agent_speed * agent_speed;
double target_speed_squared = target_velocity.getNormSquared();
double distance_squared = relative_position.getNormSquared();
if(agent_speed_squared == target_speed_squared)
{
//
// When the speeds are the same. I don't know why case
// this corresponds to, but we don't want a
// divide-by-0 error. Some of these should have
// solutions, and those will sadly be missed.
//
if(DEBUGGING_INTERSECTION_TIME)
cout << "No #3" << endl;
return NO_INTERSECTION_POSSIBLE;
}
double a = target_speed_squared - agent_speed_squared;
double b = target_velocity.dotProduct(-relative_position) * -2.0;
double c = distance_squared;
if(DEBUGGING_INTERSECTION_TIME)
{
cout << "\tdistance: " << sqrt(distance_squared) << endl;
cout << "\ta: " << a << "\t==> " << (a / target_speed_squared) << endl;
cout << "\tb: " << b << "\t==> " << (b / target_speed_squared) << endl;
cout << "\tc: " << c << "\t==> " << (c / target_speed_squared) << endl;
}
double discriminant = b * b - 4 * a * c;
if(discriminant < 0.0)
{
if(DEBUGGING_INTERSECTION_TIME)
cout << "No #4" << endl;
return NO_INTERSECTION_POSSIBLE; // target is too fast
}
assert(discriminant >= 0.0);
double sqrt_discriminant = sqrt(discriminant);
assert(a != 0.0);
double time1 = (-b - sqrt_discriminant) / (a * 2.0);
if(time1 >= 0.0)
{
if(DEBUGGING_INTERSECTION_TIME)
{
cout << "time1: " << time1
<< " \tcurrent: " << TimeSystem::getFrameStartTime()
<< " \ttotal: " << (time1 + TimeSystem::getFrameStartTime()) << endl;
}
return time1; // first possible intersection
}
assert(a != 0.0);
double time2 = (-b + sqrt_discriminant) / (a * 2.0);
if(time2 >= 0.0)
{
if(DEBUGGING_INTERSECTION_TIME)
{
cout << "time2: " << time2
<< " \tcurrent: " << TimeSystem::getFrameStartTime()
<< " \ttotal: " << (time2 + TimeSystem::getFrameStartTime()) << endl;
}
return time2; // second possible intersection
}
if(DEBUGGING_INTERSECTION_TIME)
cout << "No #5" << endl;
return NO_INTERSECTION_POSSIBLE; // both intersections are in the past
}
//----------------------------------------------------------------------------
Real Curve3::GetSpeed (Real fTime) const
{
Vector3 kVelocity = GetFirstDerivative(fTime);
Real fSpeed = kVelocity.Length();
return fSpeed;
}
// avoid only when going to collide
Vector3 SteeringBehaviour :: avoid (const WorldInterface& world,
const Vector3& original_velocity,
const Vector3& sphere_center,
double sphere_radius,
double clearance,
double avoid_distance) const
{
assert(sphere_radius >= 0.0);
assert(clearance > 0.0);
assert(clearance <= avoid_distance);
if(!world.isAlive(m_id_agent) ||
original_velocity.isZero())
{
assert(invariant());
return Vector3::ZERO;
}
Vector3 agent_position = world.getPosition(m_id_agent);
double agent_radius = world.getRadius(m_id_agent);
double desired_speed = world.getShipSpeedMax(m_id_agent);
Vector3 relative_position = sphere_center - agent_position;
double radius_sum = sphere_radius + agent_radius;
if(relative_position.isNormGreaterThan(radius_sum + avoid_distance))
{
if(DEBUGGING_AVOID)
cout << "Avoid: Outside avoid distance" << endl;
assert(invariant());
return original_velocity.getTruncated(desired_speed); // too far away to worry about
}
Vector3 agent_forward = world.getForward(m_id_agent);
if(relative_position.dotProduct(agent_forward) < 0.0)
{
// past center of object; no cylinder
if(DEBUGGING_AVOID)
cout << "Avoid: Departing from object" << endl;
if(relative_position.isNormLessThan(radius_sum + clearance))
{
// we are too close, so flee and slow down
double distance_fraction = (relative_position.getNorm() - radius_sum) / clearance;
if(DEBUGGING_AVOID)
cout << "\tInside panic distance: fraction = " << distance_fraction << endl;
if(distance_fraction < 0.0)
distance_fraction = 0.0;
Vector3 interpolated = original_velocity.getNormalized() * distance_fraction +
-relative_position.getNormalized() * (1.0 - distance_fraction);
if(distance_fraction > AVOID_SPEED_FACTOR_MIN)
desired_speed *= distance_fraction;
else
desired_speed *= AVOID_SPEED_FACTOR_MIN;
if(original_velocity.isNormLessThan(desired_speed))
desired_speed = original_velocity.getNorm();
assert(invariant());
return interpolated.getCopyWithNorm(desired_speed);
}
else
{
if(DEBUGGING_AVOID)
cout << "\tPast object" << endl;
assert(invariant());
return original_velocity.getTruncated(desired_speed); // far enough past object
}
}
else
{
// have not reached center of object; check against cylinder
if(DEBUGGING_AVOID)
cout << "Avoid: Approaching object" << endl;
double distance_from_cylinder_center = relative_position.getAntiProjection(agent_forward).getNorm();
double clearance_fraction = (distance_from_cylinder_center - radius_sum) / clearance;
if(DEBUGGING_AVOID)
{
cout << "\tTo sphere: " << relative_position << endl;
cout << "\tDistance_from_cylinder_center: " << distance_from_cylinder_center << endl;
cout << "\tRadius_sum: " << radius_sum << endl;
cout << "\tClearance: " << clearance << endl;
cout << "\tFraction: " << clearance_fraction << endl;
}
if(clearance_fraction < 0.0)
{
clearance_fraction = 0.0;
if(DEBUGGING_AVOID)
cout << "\tLined up at sphere" << endl;
}
if(clearance_fraction > 1.0)
{
if(DEBUGGING_AVOID)
cout << "\tOutside cylinder" << endl;
assert(invariant());
return original_velocity; // outside of danger cylinder
}
if(DEBUGGING_AVOID)
cout << "\tModified fraction: " << clearance_fraction << endl;
assert(!original_velocity.isZero());
assert(!agent_forward.isZero());
Vector3 sideways_vector = -relative_position.getAntiProjection(agent_forward);
while(sideways_vector.isNormLessThan(AVOID_SIDEWAYS_NORM_MIN))
{
// we have almost no sideways, so use a random value
sideways_vector = Vector3::getRandomUnitVector().getAntiProjection(agent_forward);
// keep trying until we get a good one
}
assert(!original_velocity.isZero());
assert(!sideways_vector.isZero());
assert(sideways_vector.isOrthogonal(agent_forward));
if(DEBUGGING_AVOID)
cout << "\tSideways: " << sideways_vector << endl;
Vector3 interpolated = original_velocity.getNormalized() * clearance_fraction +
sideways_vector .getNormalized() * (1.0 - clearance_fraction);
if(original_velocity.isNormLessThan(desired_speed))
desired_speed = original_velocity.getNorm();
if(DEBUGGING_AVOID)
cout << "\tDodging out of cylinder: " << interpolated.getCopyWithNorm(desired_speed) << endl;
assert(invariant());
return interpolated.getCopyWithNorm(desired_speed);
}
}
bool PhysicsDirectSpaceStateSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, ShapeRestInfo *r_info) {
ShapeSW *shape = static_cast<PhysicsServerSW *>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
ERR_FAIL_COND_V(!shape, false);
AABB aabb = p_xform.xform(shape->get_aabb());
aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size)); //motion
aabb = aabb.grow(p_margin);
/*
if (p_motion!=Vector3())
print_line(p_motion);
*/
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
real_t best_safe = 1;
real_t best_unsafe = 1;
Transform xform_inv = p_xform.affine_inverse();
MotionShapeSW mshape;
mshape.shape = shape;
mshape.motion = xform_inv.basis.xform(p_motion);
bool best_first = true;
Vector3 closest_A, closest_B;
for (int i = 0; i < amount; i++) {
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask))
continue;
if (p_exclude.has(space->intersection_query_results[i]->get_self()))
continue; //ignore excluded
const CollisionObjectSW *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i];
Vector3 point_A, point_B;
Vector3 sep_axis = p_motion.normalized();
Transform col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
if (CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
//print_line("failed motion cast (no collision)");
continue;
}
//test initial overlap
sep_axis = p_motion.normalized();
if (!CollisionSolverSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
//print_line("failed motion cast (no collision)");
return false;
}
//just do kinematic solving
real_t low = 0;
real_t hi = 1;
Vector3 mnormal = p_motion.normalized();
for (int i = 0; i < 8; i++) { //steps should be customizable..
real_t ofs = (low + hi) * 0.5;
Vector3 sep = mnormal; //important optimization for this to work fast enough
mshape.motion = xform_inv.basis.xform(p_motion * ofs);
Vector3 lA, lB;
bool collided = !CollisionSolverSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep);
if (collided) {
//print_line(itos(i)+": "+rtos(ofs));
hi = ofs;
} else {
point_A = lA;
point_B = lB;
low = ofs;
}
}
if (low < best_safe) {
best_first = true; //force reset
best_safe = low;
best_unsafe = hi;
}
if (r_info && (best_first || (point_A.distance_squared_to(point_B) < closest_A.distance_squared_to(closest_B) && low <= best_safe))) {
closest_A = point_A;
closest_B = point_B;
r_info->collider_id = col_obj->get_instance_id();
r_info->rid = col_obj->get_self();
r_info->shape = shape_idx;
r_info->point = closest_B;
r_info->normal = (closest_A - closest_B).normalized();
best_first = false;
if (col_obj->get_type() == CollisionObjectSW::TYPE_BODY) {
const BodySW *body = static_cast<const BodySW *>(col_obj);
r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(body->get_transform().origin - closest_B);
}
}
}
p_closest_safe = best_safe;
p_closest_unsafe = best_unsafe;
return true;
}
Ref<Mesh> Mesh::create_outline(float p_margin) const {
Array arrays;
int index_accum=0;
for(int i=0;i<get_surface_count();i++) {
if (surface_get_primitive_type(i)!=PRIMITIVE_TRIANGLES)
continue;
Array a = surface_get_arrays(i);
int vcount=0;
if (i==0) {
arrays=a;
DVector<Vector3> v=a[ARRAY_VERTEX];
index_accum+=v.size();
} else {
for(int j=0;j<arrays.size();j++) {
if (arrays[j].get_type()==Variant::NIL || a[j].get_type()==Variant::NIL) {
//mismatch, do not use
arrays[j]=Variant();
continue;
}
switch(j) {
case ARRAY_VERTEX:
case ARRAY_NORMAL: {
DVector<Vector3> dst = arrays[j];
DVector<Vector3> src = a[j];
if (j==ARRAY_VERTEX)
vcount=src.size();
if (dst.size()==0 || src.size()==0) {
arrays[j]=Variant();
continue;
}
dst.append_array(src);
arrays[j]=dst;
} break;
case ARRAY_TANGENT:
case ARRAY_BONES:
case ARRAY_WEIGHTS: {
DVector<real_t> dst = arrays[j];
DVector<real_t> src = a[j];
if (dst.size()==0 || src.size()==0) {
arrays[j]=Variant();
continue;
}
dst.append_array(src);
arrays[j]=dst;
} break;
case ARRAY_COLOR: {
DVector<Color> dst = arrays[j];
DVector<Color> src = a[j];
if (dst.size()==0 || src.size()==0) {
arrays[j]=Variant();
continue;
}
dst.append_array(src);
arrays[j]=dst;
} break;
case ARRAY_TEX_UV:
case ARRAY_TEX_UV2: {
DVector<Vector2> dst = arrays[j];
DVector<Vector2> src = a[j];
if (dst.size()==0 || src.size()==0) {
arrays[j]=Variant();
continue;
}
dst.append_array(src);
arrays[j]=dst;
} break;
case ARRAY_INDEX: {
DVector<int> dst = arrays[j];
DVector<int> src = a[j];
if (dst.size()==0 || src.size()==0) {
arrays[j]=Variant();
continue;
}
{
int ss = src.size();
DVector<int>::Write w = src.write();
for(int k=0;k<ss;k++) {
w[k]+=index_accum;
}
}
dst.append_array(src);
arrays[j]=dst;
index_accum+=vcount;
} break;
}
}
}
}
{
int tc=0;
DVector<int>::Write ir;
DVector<int> indices =arrays[ARRAY_INDEX];
bool has_indices=false;
DVector<Vector3> vertices =arrays[ARRAY_VERTEX];
int vc = vertices.size();
ERR_FAIL_COND_V(!vc,Ref<Mesh>());
DVector<Vector3>::Write r=vertices.write();
if (indices.size()) {
vc=indices.size();
ir=indices.write();
has_indices=true;
}
Map<Vector3,Vector3> normal_accum;
//fill normals with triangle normals
for(int i=0;i<vc;i+=3) {
Vector3 t[3];
if (has_indices) {
t[0]=r[ir[i+0]];
t[1]=r[ir[i+1]];
t[2]=r[ir[i+2]];
} else {
t[0]=r[i+0];
t[1]=r[i+1];
t[2]=r[i+2];
}
Vector3 n = Plane(t[0],t[1],t[2]).normal;
for(int j=0;j<3;j++) {
Map<Vector3,Vector3>::Element *E=normal_accum.find(t[j]);
if (!E) {
normal_accum[t[j]]=n;
} else {
float d = n.dot(E->get());
if (d<1.0)
E->get()+=n*(1.0-d);
//E->get()+=n;
}
}
}
//normalize
for (Map<Vector3,Vector3>::Element *E=normal_accum.front();E;E=E->next()) {
E->get().normalize();
}
//displace normals
int vc2 = vertices.size();
for(int i=0;i<vc2;i++) {
Vector3 t=r[i];
Map<Vector3,Vector3>::Element *E=normal_accum.find(t);
ERR_CONTINUE(!E);
t+=E->get()*p_margin;
r[i]=t;
}
r = DVector<Vector3>::Write();
arrays[ARRAY_VERTEX]=vertices;
if (!has_indices) {
DVector<int> new_indices;
new_indices.resize(vertices.size());
DVector<int>::Write iw = new_indices.write();
for(int j=0;j<vc2;j+=3) {
iw[j]=j;
iw[j+1]=j+2;
iw[j+2]=j+1;
}
iw=DVector<int>::Write();
arrays[ARRAY_INDEX]=new_indices;
} else {
for(int j=0;j<vc;j+=3) {
SWAP(ir[j+1],ir[j+2]);
}
ir=DVector<int>::Write();
arrays[ARRAY_INDEX]=indices;
}
}
Ref<Mesh> newmesh = memnew( Mesh );
newmesh->add_surface(PRIMITIVE_TRIANGLES,arrays);
return newmesh;
}
bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_pick_ray) {
ERR_FAIL_COND_V(space->locked, false);
Vector3 begin, end;
Vector3 normal;
begin = p_from;
end = p_to;
normal = (end - begin).normalized();
int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, SpaceSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
//todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
bool collided = false;
Vector3 res_point, res_normal;
int res_shape;
const CollisionObjectSW *res_obj;
real_t min_d = 1e10;
for (int i = 0; i < amount; i++) {
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask))
continue;
if (p_pick_ray && !(static_cast<CollisionObjectSW *>(space->intersection_query_results[i])->is_ray_pickable()))
continue;
if (p_exclude.has(space->intersection_query_results[i]->get_self()))
continue;
const CollisionObjectSW *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i];
Transform inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
Vector3 local_from = inv_xform.xform(begin);
Vector3 local_to = inv_xform.xform(end);
const ShapeSW *shape = col_obj->get_shape(shape_idx);
Vector3 shape_point, shape_normal;
if (shape->intersect_segment(local_from, local_to, shape_point, shape_normal)) {
Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
shape_point = xform.xform(shape_point);
real_t ld = normal.dot(shape_point);
if (ld < min_d) {
min_d = ld;
res_point = shape_point;
res_normal = inv_xform.basis.xform_inv(shape_normal).normalized();
res_shape = shape_idx;
res_obj = col_obj;
collided = true;
}
}
}
if (!collided)
return false;
r_result.collider_id = res_obj->get_instance_id();
if (r_result.collider_id != 0)
r_result.collider = ObjectDB::get_instance(r_result.collider_id);
else
r_result.collider = NULL;
r_result.normal = res_normal;
r_result.position = res_point;
r_result.rid = res_obj->get_self();
r_result.shape = res_shape;
return true;
}
void GLCanvas::renderSkeleton()
{
Vector3 lookAt = _cameraPosition + _cameraFront;
gluLookAt(_cameraPosition.x(), _cameraPosition.y(), _cameraPosition.z(),
lookAt.x() , lookAt.y() , lookAt.z(),
_cameraUp.x() , _cameraUp.y() , _cameraUp.z());
if (_style & DRAW_GRID)
{
drawGrid();
}
if (_skeleton == nullptr)
{
return;
}
for (auto it = _skeleton->beginBones(); it != _skeleton->endBones(); ++it)
{
glPushMatrix();
Bone bone = *(it->second);
int boneId = bone.getId();
// don't draw any bone without valid id
if (boneId < 0)
{
continue;
}
// get the bone id as color for SELECTION_MODE
GLubyte boneIdColor[4] = {GLubyte((boneId >> 8) & 255), GLubyte((boneId >> 8) & 255), GLubyte(boneId & 255), 255};
const GLubyte* boneColor1 = boneStandardColor1;
const GLubyte* boneColor2 = boneStandardColor2;
if (_style & SELECTION_MODE)
{
boneColor1 = boneIdColor;
boneColor2 = boneIdColor;
}
else if (_style & HIGHLIGHT_SELECTED_BONE && bone.getId() == _skeleton->getSelectedBoneId())
{
boneColor1 = boneHighlightedColor1;
boneColor2 = boneHighlightedColor2;
}
Vector3 startPos = bone.getStartPos();
glTranslatef(startPos.x(), startPos.y(), startPos.z());
float length = bone.getLength();
Vector3 dir = bone.getDirection();
Vector3 up = bone.getUpDirection();
Vector3 right = bone.getRightDirection();
Vector3 endPos = dir*length;
startPos = Vector3(0, 0, 0);
float length_10 = length * 0.1f;
Vector3 endPos_10 = endPos * 0.1f;
Vector3 upPoint = up*length_10 + endPos_10;
Vector3 downPoint = - up*length_10 + endPos_10;
Vector3 rightPoint = right*length_10 + endPos_10;
Vector3 leftPoint = - right*length_10 + endPos_10;
if (!(_style & SELECTION_MODE))
{
//set point size to 10 pixels
glPointSize(10.0f);
glColor4ubv(pointColor);
// TODO(JK#9#): maybe don't draw points for bones (or add render style flag)
glBegin(GL_POINTS);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glEnd();
}
// set line width
glLineWidth(_lineWidth);
// draw local coordinate system
if (_style & DRAW_LOCAL_COORDINATE_SYSTEM)
{
glBegin(GL_LINES);
glColor4ubv(red);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glVertex3f(endPos.x() + dir.x()*0.1f, endPos.y() + dir.y()*0.1f, endPos.z() + dir.z()*0.1f);
glColor4ubv(green);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glVertex3f(endPos.x() + up.x()*0.1f, endPos.y() + up.y()*0.1f, endPos.z() + up.z()*0.1f);
glColor4ubv(blue);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glVertex3f(endPos.x() + right.x()*0.1f, endPos.y() + right.y()*0.1f, endPos.z() + right.z()*0.1f);
glEnd();
}
if (_style & DRAW_ROTATION_AXIS)
{
Vector3 rotAxis = bone.getRelOrientation().getRotationAxis();
glBegin(GL_LINES);
glColor4ubv(yellow);
glVertex3f(-rotAxis.x()*0.2f, -rotAxis.y()*0.2f, -rotAxis.z()*0.2f);
glVertex3f(rotAxis.x()*0.2f, rotAxis.y()*0.2f, rotAxis.z()*0.2f);
glEnd();
}
// draw bone
glPolygonMode(GL_FRONT, GL_FILL);
glColor4ubv(boneColor1);
glBegin(GL_TRIANGLE_FAN);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glVertex3f(leftPoint.x(), leftPoint.y(), leftPoint.z());
glVertex3f(downPoint.x(), downPoint.y(), downPoint.z());
glVertex3f(rightPoint.x(), rightPoint.y(), rightPoint.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glEnd();
glColor4ubv(boneColor2);
glBegin(GL_TRIANGLE_FAN);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glVertex3f(rightPoint.x(), rightPoint.y(), rightPoint.z());
glVertex3f(downPoint.x(), downPoint.y(), downPoint.z());
glVertex3f(leftPoint.x(), leftPoint.y(), leftPoint.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glEnd();
if (!(_style & SELECTION_MODE))
{
// draw black mesh lines around bones
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glColor4ubv(black);
glBegin(GL_TRIANGLE_FAN);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glVertex3f(leftPoint.x(), leftPoint.y(), leftPoint.z());
glVertex3f(downPoint.x(), downPoint.y(), downPoint.z());
glVertex3f(rightPoint.x(), rightPoint.y(), rightPoint.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glEnd();
glBegin(GL_TRIANGLE_FAN);
glVertex3f(endPos.x(), endPos.y(), endPos.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glVertex3f(rightPoint.x(), rightPoint.y(), rightPoint.z());
glVertex3f(downPoint.x(), downPoint.y(), downPoint.z());
glVertex3f(leftPoint.x(), leftPoint.y(), leftPoint.z());
glVertex3f(upPoint.x(), upPoint.y(), upPoint.z());
glEnd();
// draw labels
if (_style & DRAW_LABEL)
{
// glDisable(GL_DEPTH_TEST);
GLImage* image = _labels.find(boneId)->second;
image->setPosition(0.5f * bone.getLength() * dir - 0.06f * bone.getLength() * _cameraFront);
image->render();
// glEnable(GL_DEPTH_TEST);
}
}
// reset line width
glLineWidth(1.0f);
if (_style & DRAW_SPIN_ARROWS && bone.getId() == _skeleton->getSelectedBoneId())
{
drawSpinArrows(endPos - 0.2 * bone.getLength() * dir, dir, up, right);
}
glPopMatrix();
// append trace point and draw trace
if (_traceLength > 0)
{
Vector3 globalEndPos = bone.getEndPos();
std::map<int, std::vector<Vector3> >::iterator traceIt = _boneIdsWithTracePoints.find(boneId);
if (traceIt != _boneIdsWithTracePoints.end())
{
if (traceIt->second.size() < _traceLength)
{
traceIt->second.push_back(globalEndPos);
}
else
{
traceIt->second[_tracePos] = globalEndPos;
}
glBegin(GL_LINES);
glColor4ubv(yellow);
for (size_t i = 0; i < traceIt->second.size() - 1; ++i)
{
if (i != _tracePos)
{
glVertex3f(traceIt->second[i].x(), traceIt->second[i].y(), traceIt->second[i].z());
glVertex3f(traceIt->second[i+1].x(), traceIt->second[i+1].y(), traceIt->second[i+1].z());
}
}
// draw gab between end and beginning of the vector data
if (traceIt->second.size() > 1 && _tracePos != traceIt->second.size() - 1)
{
glVertex3f(traceIt->second.back().x(), traceIt->second.back().y(), traceIt->second.back().z());
glVertex3f(traceIt->second[0].x(), traceIt->second[0].y(), traceIt->second[0].z());
}
glEnd();
}
}
}
// draw joints (after everything else, as they are transparent and need everything else be rendered)
if (_style & DRAW_JOINT_CONSTRAINTS)
{
for (auto it = _skeleton->beginBones(); it != _skeleton->endBones(); ++it)
{
glPushMatrix();
Bone bone = *(it->second);
Vector3 startPos = bone.getStartPos();
glTranslatef(startPos.x(), startPos.y(), startPos.z());
JointConstraint constraint = bone.getJointConstraint();
drawJoint(constraint);
glPopMatrix();
}
}
// update trace position
if (++_tracePos >= _traceLength)
{
_tracePos = 0;
}
if (!(_style & SELECTION_MODE) && _style & DRAW_AABB)
{
drawAABB(_skeleton->getAABB());
}
}
void GLCanvas::drawSpinArrows(Vector3 pos, Vector3 dir, Vector3 up, Vector3 right) const
{
float length = 0.03f;
float offset = 0.05f;
GLubyte red[4] = {255, 0, 0, 255};
GLubyte green[4] = {0, 255, 0, 255};
GLubyte blue[4] = {0, 0, 255, 255};
GLubyte black[4] = {0, 0, 0, 255};
// set arroe ids to next available free ids
int idArrowDir = _skeleton->getNextFreeId();
int idArrowUp = _skeleton->getNextFreeId() + 1;
int idArrowRight = _skeleton->getNextFreeId() + 2;
GLubyte idArrowDirColor[4] = {GLubyte((idArrowDir >> 8) & 255), GLubyte((idArrowDir >> 8) & 255), GLubyte(idArrowDir & 255), 255};
GLubyte idArrowUpColor[4] = {GLubyte((idArrowUp >> 8) & 255), GLubyte((idArrowUp >> 8) & 255), GLubyte(idArrowUp & 255), 255};
GLubyte idArrowRightColor[4] = {GLubyte((idArrowRight >> 8) & 255), GLubyte((idArrowRight >> 8) & 255), GLubyte(idArrowRight & 255), 255};
const GLubyte* arrowDirColor = red;
const GLubyte* arrowUpColor = green;
const GLubyte* arrowRightColor = blue;
// base points of the arrow pyramids
Vector3 basePointUp = pos + up * offset;
Vector3 basePointRight = pos + right * offset;
Vector3 basePointDown = pos - up * offset;
Vector3 basePointLeft = pos - right * offset;
dir *= length;
up *= length;
right *= length;
Vector3 dirHalf = dir * 0.5;
Vector3 rightHalf = right * 0.5;
// base vertices of the circle like arrow
Vector3 upperCircle[4];
upperCircle[0] = basePointDown;
upperCircle[1] = basePointDown - right;
upperCircle[2] = basePointLeft - up;
upperCircle[3] = basePointLeft;
Vector3 lowerCircle[4];
lowerCircle[0] = basePointDown - up;
lowerCircle[1] = basePointDown - right*1.4 - up;
lowerCircle[2] = basePointLeft - right - up*1.4;
lowerCircle[3] = basePointLeft - right;
// the arrow rendering is done twice, one iteration for filling color, one for having black corner lines
int numIterations = 2;
if (_style & SELECTION_MODE)
{
// do not draw the corner lines in selection mode
numIterations = 1;
// draw the arrows with their id color
arrowDirColor = idArrowDirColor;
arrowUpColor = idArrowUpColor;
arrowRightColor = idArrowRightColor;
}
// draw vertices twice, one run with filling and one with black lines
for (int j = 0; j < numIterations; ++j)
{
if (j == 0)
{
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
}
else
{
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glColor4f(0.0, 0.0, 0.0, 1.0);
arrowDirColor = arrowUpColor = arrowRightColor = black;
}
if (j == 0)
{
glColor4ubv(arrowUpColor);
}
glColor4ubv(arrowUpColor);
// arrow pointing upwards
glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointUp.x() + up.x()*2.0, basePointUp.y() + up.y()*2.0, basePointUp.z() + up.z()*2.0);
glVertex3f(basePointUp.x() + dir.x(), basePointUp.y() + dir.y(), basePointUp.z() + dir.z());
glVertex3f(basePointUp.x() + right.x(), basePointUp.y() + right.y(), basePointUp.z() + right.z());
glVertex3f(basePointUp.x() - dir.x(), basePointUp.y() - dir.y(), basePointUp.z() - dir.z());
glVertex3f(basePointUp.x() - right.x(), basePointUp.y() - right.y(), basePointUp.z() - right.z());
glVertex3f(basePointUp.x() + dir.x(), basePointUp.y() + dir.y(), basePointUp.z() + dir.z());
glEnd();
// arrow pointing downwards
/* glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointDown.x() - up.x(), basePointDown.y() - up.y(), basePointDown.z() - up.z());
glVertex3f(basePointDown.x() + dir.x(), basePointDown.y() + dir.y(), basePointDown.z() + dir.z());
glVertex3f(basePointDown.x() + right.x(), basePointDown.y() + right.y(), basePointDown.z() + right.z());
glVertex3f(basePointDown.x() - dir.x(), basePointDown.y() - dir.y(), basePointDown.z() - dir.z());
glVertex3f(basePointDown.x() - right.x(), basePointDown.y() - right.y(), basePointDown.z() - right.z());
glVertex3f(basePointDown.x() + dir.x(), basePointDown.y() + dir.y(), basePointDown.z() + dir.z());
glEnd();
*/
if (j == 0)
{
glColor4f(0.0, 0.0, 1.0, 1.0);
}
glColor4ubv(arrowRightColor);
// arrow pointing to the right
glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointRight.x() + right.x()*2.0, basePointRight.y() + right.y()*2.0, basePointRight.z() + right.z()*2.0);
glVertex3f(basePointRight.x() + dir.x(), basePointRight.y() + dir.y(), basePointRight.z() + dir.z());
glVertex3f(basePointRight.x() - up.x(), basePointRight.y() - up.y(), basePointRight.z() - up.z());
glVertex3f(basePointRight.x() - dir.x(), basePointRight.y() - dir.y(), basePointRight.z() - dir.z());
glVertex3f(basePointRight.x() + up.x(), basePointRight.y() + up.y(), basePointRight.z() + up.z());
glVertex3f(basePointRight.x() + dir.x(), basePointRight.y() + dir.y(), basePointRight.z() + dir.z());
glEnd();
// arrow pointing to the left
/* glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointLeft.x() - right.x(), basePointLeft.y() - right.y(), basePointLeft.z() - right.z());
glVertex3f(basePointLeft.x() + dir.x(), basePointLeft.y() + dir.y(), basePointLeft.z() + dir.z());
glVertex3f(basePointLeft.x() - up.x(), basePointLeft.y() - up.y(), basePointLeft.z() - up.z());
glVertex3f(basePointLeft.x() - dir.x(), basePointLeft.y() - dir.y(), basePointLeft.z() - dir.z());
glVertex3f(basePointLeft.x() + up.x(), basePointLeft.y() + up.y(), basePointLeft.z() + up.z());
glVertex3f(basePointLeft.x() + dir.x(), basePointLeft.y() + dir.y(), basePointLeft.z() + dir.z());
glEnd();
*/
if (j == 0)
{
glColor4f(1.0, 0.0, 0.0, 1.0);
}
glColor4ubv(arrowDirColor);
glBegin(GL_TRIANGLE_FAN);
// top of arrow
glVertex3f(basePointLeft.x() - rightHalf.x() + up.x()*2.0, basePointLeft.y() - rightHalf.y() + up.y()*2.0, basePointLeft.z() - rightHalf.z() + up.z()*2.0);
glVertex3f(basePointLeft.x() + rightHalf.x(), basePointLeft.y() + rightHalf.y(), basePointLeft.z() + rightHalf.z());
glVertex3f(basePointLeft.x() - rightHalf.x() + dir.x(), basePointLeft.y() - rightHalf.y() + dir.y(), basePointLeft.z() - rightHalf.z() + dir.z());
glVertex3f(basePointLeft.x() - right.x() - rightHalf.x(), basePointLeft.y() - right.y() - rightHalf.y(), basePointLeft.z() - right.z() - rightHalf.z());
glVertex3f(basePointLeft.x() - rightHalf.x() - dir.x(), basePointLeft.y() - rightHalf.y() - dir.y(), basePointLeft.z() - rightHalf.z() - dir.z());
glVertex3f(basePointLeft.x() + rightHalf.x(), basePointLeft.y() + rightHalf.y(), basePointLeft.z() + rightHalf.z());
glEnd();
// draw arrows base
glBegin(GL_QUAD_STRIP);
for (int i = 0; i < 4; ++i)
{
glVertex3f(upperCircle[i].x() + dirHalf.x(), upperCircle[i].y() + dirHalf.y(), upperCircle[i].z() + dirHalf.z());
glVertex3f(upperCircle[i].x() - dirHalf.x(), upperCircle[i].y() - dirHalf.y(), upperCircle[i].z() - dirHalf.z());
}
for (int i = 3; i >= 0; --i)
{
glVertex3f(lowerCircle[i].x() + dirHalf.x(), lowerCircle[i].y() + dirHalf.y(), lowerCircle[i].z() + dirHalf.z());
glVertex3f(lowerCircle[i].x() - dirHalf.x(), lowerCircle[i].y() - dirHalf.y(), lowerCircle[i].z() - dirHalf.z());
}
glVertex3f(upperCircle[0].x() + dirHalf.x(), upperCircle[0].y() + dirHalf.y(), upperCircle[0].z() + dirHalf.z());
glVertex3f(upperCircle[0].x() - dirHalf.x(), upperCircle[0].y() - dirHalf.y(), upperCircle[0].z() - dirHalf.z());
glEnd();
glBegin(GL_QUAD_STRIP);
for (int i = 0; i < 4; ++i)
{
glVertex3f(lowerCircle[i].x() + dirHalf.x(), lowerCircle[i].y() + dirHalf.y(), lowerCircle[i].z() + dirHalf.z());
glVertex3f(upperCircle[i].x() + dirHalf.x(), upperCircle[i].y() + dirHalf.y(), upperCircle[i].z() + dirHalf.z());
}
for (int i = 3; i >= 0; --i)
{
glVertex3f(lowerCircle[i].x() - dirHalf.x(), lowerCircle[i].y() - dirHalf.y(), lowerCircle[i].z() - dirHalf.z());
glVertex3f(upperCircle[i].x() - dirHalf.x(), upperCircle[i].y() - dirHalf.y(), upperCircle[i].z() - dirHalf.z());
}
glEnd();
}
/* glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glColor4f(0.0, 0.0, 0.0, 1.0);
// arrow pointing upwards
glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointUp.x() + up.x()*2.0, basePointUp.y() + up.y()*2.0, basePointUp.z() + up.z()*2.0);
glVertex3f(basePointUp.x() + dir.x(), basePointUp.y() + dir.y(), basePointUp.z() + dir.z());
glVertex3f(basePointUp.x() + right.x(), basePointUp.y() + right.y(), basePointUp.z() + right.z());
glVertex3f(basePointUp.x() - dir.x(), basePointUp.y() - dir.y(), basePointUp.z() - dir.z());
glVertex3f(basePointUp.x() - right.x(), basePointUp.y() - right.y(), basePointUp.z() - right.z());
glVertex3f(basePointUp.x() + dir.x(), basePointUp.y() + dir.y(), basePointUp.z() + dir.z());
glEnd();
// arrow pointing to the right
glBegin(GL_TRIANGLE_FAN);
glVertex3f(basePointRight.x() + right.x()*2.0, basePointRight.y() + right.y()*2.0, basePointRight.z() + right.z()*2.0);
glVertex3f(basePointRight.x() + dir.x(), basePointRight.y() + dir.y(), basePointRight.z() + dir.z());
glVertex3f(basePointRight.x() - up.x(), basePointRight.y() - up.y(), basePointRight.z() - up.z());
glVertex3f(basePointRight.x() - dir.x(), basePointRight.y() - dir.y(), basePointRight.z() - dir.z());
glVertex3f(basePointRight.x() + up.x(), basePointRight.y() + up.y(), basePointRight.z() + up.z());
glVertex3f(basePointRight.x() + dir.x(), basePointRight.y() + dir.y(), basePointRight.z() + dir.z());
glEnd();*/
}
void GLCanvas::renderSingleSensor() const
{
// Vector3 lookAt = Vector3(0.0f, 0.0f, 0.0f);
// Vector3 lookAt = _cameraPosition + _cameraFront;
// gluLookAt(_cameraPosition.x(), _cameraPosition.y(), _cameraPosition.z(),
// lookAt.x() , lookAt.y() , lookAt.z(),
// _cameraUp.x() , _cameraUp.y() , _cameraUp.z());
gluLookAt(0.0f, 0.0f, _cameraPosition.z(),
0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
Vector3 dir = _sensorOrientation.rotate(Vector3(1.0f, 0.0f, 0.0f));
Vector3 up = 0.25 * _sensorOrientation.rotate(Vector3(0.0f, 1.0f, 0.0f));
Vector3 right = 0.5 * _sensorOrientation.rotate(Vector3(0.0f, 0.0f, 1.0f));
Vector3 frontUpRight = dir + up + right;
Vector3 frontDownRight = dir - up + right;
Vector3 frontUpLeft = dir + up - right;
Vector3 frontDownLeft = dir - up - right;
Vector3 backUpRight = -dir + up + right;
Vector3 backDownRight = -dir - up + right;
Vector3 backUpLeft = -dir + up - right;
Vector3 backDownLeft = -dir - up - right;
glBegin(GL_QUADS);
glColor3f(0.8, 0.5, 0.5);
glVertex3f(frontUpLeft.x(), frontUpLeft.y(), frontUpLeft.z());
glVertex3f(frontUpRight.x(), frontUpRight.y(), frontUpRight.z());
glVertex3f(frontDownRight.x(), frontDownRight.y(), frontDownRight.z());
glVertex3f(frontDownLeft.x(), frontDownLeft.y(), frontDownLeft.z());
glColor3f(0.5, 0.7, 0.5);
glVertex3f(backUpRight.x(), backUpRight.y(), backUpRight.z());
glVertex3f(frontUpRight.x(), frontUpRight.y(), frontUpRight.z());
glVertex3f(frontUpLeft.x(), frontUpLeft.y(), frontUpLeft.z());
glVertex3f(backUpLeft.x(), backUpLeft.y(), backUpLeft.z());
glColor3f(0.5, 0.5, 0.7);
glVertex3f(backDownRight.x(), backDownRight.y(), backDownRight.z());
glVertex3f(frontDownRight.x(), frontDownRight.y(), frontDownRight.z());
glVertex3f(frontUpRight.x(), frontUpRight.y(), frontUpRight.z());
glVertex3f(backUpRight.x(), backUpRight.y(), backUpRight.z());
glColor3f(0.7, 0.7, 0.7);
glVertex3f(backUpRight.x(), backUpRight.y(), backUpRight.z());
glVertex3f(backUpLeft.x(), backUpLeft.y(), backUpLeft.z());
glVertex3f(backDownLeft.x(), backDownLeft.y(), backDownLeft.z());
glVertex3f(backDownRight.x(), backDownRight.y(), backDownRight.z());
glVertex3f(backDownLeft.x(), backDownLeft.y(), backDownLeft.z());
glVertex3f(frontDownLeft.x(), frontDownLeft.y(), frontDownLeft.z());
glVertex3f(frontDownRight.x(), frontDownRight.y(), frontDownRight.z());
glVertex3f(backDownRight.x(), backDownRight.y(), backDownRight.z());
glVertex3f(backUpLeft.x(), backUpLeft.y(), backUpLeft.z());
glVertex3f(frontUpLeft.x(), frontUpLeft.y(), frontUpLeft.z());
glVertex3f(frontDownLeft.x(), frontDownLeft.y(), frontDownLeft.z());
glVertex3f(backDownLeft.x(), backDownLeft.y(), backDownLeft.z());
glEnd();
// scale slightly to ensure the lines are visible
glScalef(1.001f, 1.001f, 1.001f);
glLineWidth(1.0f);
glColor3f(0.0, 0.0, 0.0);
glBegin(GL_LINE_STRIP);
glVertex3f(backUpRight.x(), backUpRight.y(), backUpRight.z());
glVertex3f(backUpLeft.x(), backUpLeft.y(), backUpLeft.z());
glVertex3f(backDownLeft.x(), backDownLeft.y(), backDownLeft.z());
glVertex3f(backDownRight.x(), backDownRight.y(), backDownRight.z());
glVertex3f(backUpRight.x(), backUpRight.y(), backUpRight.z());
glVertex3f(frontUpRight.x(), frontUpRight.y(), frontUpRight.z());
glVertex3f(frontUpLeft.x(), frontUpLeft.y(), frontUpLeft.z());
glVertex3f(frontDownLeft.x(), frontDownLeft.y(), frontDownLeft.z());
glVertex3f(frontDownRight.x(), frontDownRight.y(), frontDownRight.z());
glVertex3f(frontUpRight.x(), frontUpRight.y(), frontUpRight.z());
glEnd();
glBegin(GL_LINES);
glVertex3f(backUpLeft.x(), backUpLeft.y(), backUpLeft.z());
glVertex3f(frontUpLeft.x(), frontUpLeft.y(), frontUpLeft.z());
glVertex3f(backDownLeft.x(), backDownLeft.y(), backDownLeft.z());
glVertex3f(frontDownLeft.x(), frontDownLeft.y(), frontDownLeft.z());
glVertex3f(backDownRight.x(), backDownRight.y(), backDownRight.z());
glVertex3f(frontDownRight.x(), frontDownRight.y(), frontDownRight.z());
glEnd();
glLineWidth(2.0f);
// draw global coordinate system
glBegin(GL_LINES);
glColor3f(1.0, 0.0, 0.0);
glVertex3f(-1.5, -1.0, -1.0);
glVertex3f(1.0, -1.0, -1.0);
glColor3f(0.0, 1.0, 0.0);
glVertex3f(-1.5, -1.0, -1.0);
glVertex3f(-1.5, 1.0, -1.0);
glColor3f(0.0, 0.0, 1.0);
glVertex3f(-1.5, -1.0, -1.0);
glVertex3f(-1.5, -1.0, 1.0);
glEnd();
// draw global coordinate system
// glBegin(GL_LINES);
// glColor3f(1.0, 0.0, 0.0);
// glVertex3f(0.0, 0.0, 0.0);
// glVertex3f(1.5, 00.0, 0.0);
//
// glColor3f(0.0, 1.0, 0.0);
// glVertex3f(0.0, 0.0, 0.0);
// glVertex3f(0.0, 1.5, 0.0);
//
// glColor3f(0.0, 0.0, 1.0);
// glVertex3f(0.0, 0.0, 0.0);
// glVertex3f(0.0, 0.0, 1.5);
// glEnd();
glLineWidth(1.0f);
}
void GLCanvas::renderSingleJoint() const
{
Vector3 lookAt = _cameraPosition + _cameraFront;
gluLookAt(_cameraPosition.x(), _cameraPosition.y(), _cameraPosition.z(),
lookAt.x() , lookAt.y() , lookAt.z(),
_cameraUp.x() , _cameraUp.y() , _cameraUp.z());
glBegin(GL_LINES);
glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(1.0f, 0.0f, 0.0f);
glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, 1.0f, 0.0f);
glColor4f(0.0f, 0.0f, 1.0f, 1.0f);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, 0.0f, 1.0f);
glEnd();
if (_constraint != nullptr)
{
drawJoint(*_constraint, 1.0f);
}
}
// Initialization of all OpenGL specific parameters.
void GLCanvas::InitGL()
{
SetCurrent(*_GLRC);
glClearColor(0.0, 0.0, 0.0, 0.0);
//glClearColor(1.0, 1.0, 1.0, 1.0);
glEnable(GL_DEPTH_TEST);
glEnable(GL_POINT_SMOOTH);
glEnable(GL_BLEND);
glEnable(GL_CULL_FACE);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glShadeModel(GL_SMOOTH);
}
GeometryT<T> GeometryT<T>::translate(Vector3<T> v) {
Vector3<T> withScale = v.applyScale();
return this->translate(withScale.getX(), withScale.getY());
}
// Return true if the triangle is visible from a given vertex
inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const {
Vector3 closestToVert = vertices[index] - mClosestPoint;
return (mClosestPoint.dot(closestToVert) > 0.0);
}
/**
* Create a sequence of flag objects and add them to the world.
*/
void createFlag( int width, int height, btAlignedObjectArray<btSoftBody *> &flags )
{
// First create a triangle mesh to represent a flag
using namespace BTAcceleratedSoftBody;
using Vectormath::Aos::Matrix3;
using Vectormath::Aos::Vector3;
// Allocate a simple mesh consisting of a vertex array and a triangle index array
btIndexedMesh mesh;
mesh.m_numVertices = width*height;
mesh.m_numTriangles = 2*(width-1)*(height-1);
btVector3 *vertexArray = new btVector3[mesh.m_numVertices];
mesh.m_vertexBase = reinterpret_cast<const unsigned char*>(vertexArray);
int *triangleVertexIndexArray = new int[3*mesh.m_numTriangles];
mesh.m_triangleIndexBase = reinterpret_cast<const unsigned char*>(triangleVertexIndexArray);
mesh.m_triangleIndexStride = sizeof(int)*3;
mesh.m_vertexStride = sizeof(Vector3);
// Generate normalised object space vertex coordinates for a rectangular flag
float zCoordinate = 0.0f;
Matrix3 defaultScale(Vector3(5.f, 0.f, 0.f), Vector3(0.f, 20.f, 0.f), Vector3(0.f, 0.f, 1.f));
for( int y = 0; y < height; ++y )
{
float yCoordinate = y*2.0f/float(height) - 1.0f;
for( int x = 0; x < width; ++x )
{
float xCoordinate = x*2.0f/float(width) - 1.0f;
Vector3 vertex(xCoordinate, yCoordinate, zCoordinate);
Vector3 transformedVertex = defaultScale*vertex;
vertexArray[y*width + x] = btVector3(transformedVertex.getX(), transformedVertex.getY(), transformedVertex.getZ() );
}
}
// Generate vertex indices for triangles
for( int y = 0; y < (height-1); ++y )
{
for( int x = 0; x < (width-1); ++x )
{
// Triangle 0
// Top left of square on mesh
{
int vertex0 = y*width + x;
int vertex1 = vertex0 + 1;
int vertex2 = vertex0 + width;
int triangleIndex = 2*y*(width-1) + 2*x;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)] = vertex0;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex+1)/sizeof(int)+1] = vertex1;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex+2)/sizeof(int)+2] = vertex2;
}
// Triangle 1
// Bottom right of square on mesh
{
int vertex0 = y*width + x + 1;
int vertex1 = vertex0 + width;
int vertex2 = vertex1 - 1;
int triangleIndex = 2*y*(width-1) + 2*x + 1;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)] = vertex0;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)+1] = vertex1;
triangleVertexIndexArray[(mesh.m_triangleIndexStride*triangleIndex)/sizeof(int)+2] = vertex2;
}
}
}
float rotateAngleRoundZ = 0.5;
float rotateAngleRoundX = 0.5;
btMatrix3x3 defaultRotate;
defaultRotate[0] = btVector3(cos(rotateAngleRoundZ), sin(rotateAngleRoundZ), 0.f);
defaultRotate[1] = btVector3(-sin(rotateAngleRoundZ), cos(rotateAngleRoundZ), 0.f);
defaultRotate[2] = btVector3(0.f, 0.f, 1.f);
//btMatrix3x3 defaultRotateAndScale( (defaultRotateX*defaultRotate) );
#ifdef TABLETEST
btMatrix3x3 defaultRotateX;
rotateAngleRoundX = 3.141592654/2;
defaultRotateX[0] = btVector3(1.f, 0.f, 0.f);
defaultRotateX[1] = btVector3( 0.f, cos(rotateAngleRoundX), sin(rotateAngleRoundX));
defaultRotateX[2] = btVector3(0.f, -sin(rotateAngleRoundX), cos(rotateAngleRoundX));
btMatrix3x3 defaultRotateAndScale( (defaultRotateX) );
#else
btMatrix3x3 defaultRotateX;
defaultRotateX[0] = btVector3(1.f, 0.f, 0.f);
defaultRotateX[1] = btVector3( 0.f, cos(rotateAngleRoundX), sin(rotateAngleRoundX));
defaultRotateX[2] = btVector3(0.f, -sin(rotateAngleRoundX), cos(rotateAngleRoundX));
btMatrix3x3 defaultRotateAndScale( (defaultRotateX) );
#endif
// Construct the sequence flags applying a slightly different translation to each one to arrange them
// appropriately in the scene.
for( int i = 0; i < numFlags; ++i )
{
float zTranslate = flagSpacing * (i-numFlags/2);
btVector3 defaultTranslate(0.f, 20.f, zTranslate);
btTransform transform( defaultRotateAndScale, defaultTranslate );
btSoftBody *softBody = createFromIndexedMesh( vertexArray, mesh.m_numVertices, triangleVertexIndexArray, mesh.m_numTriangles, true );
for( int i = 0; i < mesh.m_numVertices; ++i )
{
softBody->setMass(i, 10.f/mesh.m_numVertices);
}
#ifndef TABLETEST
// Set the fixed points
softBody->setMass((height-1)*(width), 0.f);
softBody->setMass((height-1)*(width) + width - 1, 0.f);
softBody->setMass((height-1)*width + width/2, 0.f);
#endif
softBody->m_cfg.collisions = btSoftBody::fCollision::CL_SS+btSoftBody::fCollision::CL_RS;
softBody->m_cfg.kLF = 0.0005f;
softBody->m_cfg.kVCF = 0.001f;
softBody->m_cfg.kDP = 0.f;
softBody->m_cfg.kDG = 0.f;
flags.push_back( softBody );
softBody->transform( transform );
softBody->setFriction( 0.8f );
m_dynamicsWorld->addSoftBody( softBody );
}
delete [] vertexArray;
delete [] triangleVertexIndexArray;
}
void HalfSpace3<Real>::SetNormal (const Vector3<Real>& rkNormal)
{
m_afTuple[0] = rkNormal.X();
m_afTuple[1] = rkNormal.Y();
m_afTuple[2] = rkNormal.Z();
}
double Cube::value(const Vector3 &pos) const
{
// This is a really expensive operation and so should be avoided
// Interpolate the value at the supplied vector - trilinear interpolation...
Vector3 delta = pos - m_min;
// Find the integer low and high corners
Vector3i lC(static_cast<int>(delta.x() / m_spacing.x()),
static_cast<int>(delta.y() / m_spacing.y()),
static_cast<int>(delta.z() / m_spacing.z()));
Vector3i hC(lC.x() + 1,
lC.y() + 1,
lC.z() + 1);
// So there are six corners in total - work out the delta of the position
// and the low corner
Vector3 P((delta.x() - lC.x()*m_spacing.x()) / m_spacing.x(),
(delta.y() - lC.y()*m_spacing.y()) / m_spacing.y(),
(delta.z() - lC.z()*m_spacing.z()) / m_spacing.z());
Vector3 dP = Vector3(1.0, 1.0, 1.0) - P;
// Now calculate and return the interpolated value
return value(lC.x(), lC.y(), lC.z()) * dP.x() * dP.y() * dP.z() +
value(hC.x(), lC.y(), lC.z()) * P.x() * dP.y() * dP.z() +
value(lC.x(), hC.y(), lC.z()) * dP.x() * P.y() * dP.z() +
value(lC.x(), lC.y(), hC.z()) * dP.x() * dP.y() * P.z() +
value(hC.x(), lC.y(), hC.z()) * P.x() * dP.y() * P.z() +
value(lC.x(), hC.y(), hC.z()) * dP.x() * P.y() * P.z() +
value(hC.x(), hC.y(), lC.z()) * P.x() * P.y() * dP.z() +
value(hC.x(), hC.y(), hC.z()) * P.x() * P.y() * P.z();
}
void PhysicsMesh::InitializeMesh(const std::vector<Vector3>& mesh, float radius)
{
float lenSq;
//if we have an arbitrary mesh
if (mesh.size())
{
radius_ = 0.0f;
mesh_ = mesh;
//loop through the mesh and find out extents of box and farthest point.
for (auto& it : mesh)
{
//farthest point len will be radius
lenSq = it.LengthSq();
if (lenSq > radius_)
{
radius_ = lenSq;
}
//greatest x,y,z
if (it.x > bounding_box_.max_.x)
{
bounding_box_.max_.x = it.x;
}
if (it.y > bounding_box_.max_.y)
{
bounding_box_.max_.y = it.y;
}
if (it.z > bounding_box_.max_.z)
{
bounding_box_.max_.z = it.z;
}
//least x,y,z
if (it.x < bounding_box_.min_.x)
{
bounding_box_.min_.x = it.x;
}
if (it.y < bounding_box_.min_.y)
{
bounding_box_.min_.y = it.y;
}
if (it.z < bounding_box_.min_.z)
{
bounding_box_.min_.z = it.z;
}
}
//save sqrt till end
radius_ = sqrt(radius_);
}
else if (radius)
{
radius_ = radius;
bounding_box_.max_.x = bounding_box_.max_.y = bounding_box_.max_.z = radius_;
bounding_box_.min_.x = bounding_box_.min_.y = bounding_box_.min_.z = -radius_;
}
else
{
Vector3 radVecFromBox = bounding_box_.max_ - bounding_box_.min_;
radVecFromBox *= 0.5f;
radius_ = radVecFromBox.Length();
}
}
// @brief : カメラ行列の作成
//--------------------------------------------------------------------
Matrix3D Camera::Matrix( void )const
{
const Vector3 at = At;
const Vector3 eye = Eye;
const Vector3 up = Up;
#if 0
Vector3 forward;
forward = at- eye;
forward.Normalize();
Vector3 side = forward.Cross(up);
side.Normalize();
Vector3 uper = side.Cross(forward);
uper.Normalize();
return Matrix3D(
side.x,uper.x,-forward.x,0,
side.y,uper.y,-forward.y,0,
side.z,uper.z,-forward.z,0,
0,0,0,1
);
#else
const Vector3 z(Vector3(eye-at).Normalize());
const Vector3 x(up.Cross(z).Normalize());
const Vector3 y(z.Cross(x).Normalize());
return Matrix3D (
x.x, x.y, x.z, -x.Dot(eye),
y.x, y.y, y.z, -y.Dot(eye),
z.x, z.y, z.z, -z.Dot(eye),
0.0f,0.0f,0.0f, 1.0f
);
#endif
}
float SteeringVehicle::calcDistance(const Vector3& vec1, const Vector3& vec2)
{
Vector3 vec = vec1-vec2;
return vec.length();
}