Esempio n. 1
0
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;
};
Esempio n. 2
0
 Vector3 operator*(const Matrix4 &lhs, const Vector3 &rhs)
 {
    return Vector3(rhs.Transform(lhs));
 }
Esempio n. 3
0
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_);
}
Esempio n. 4
0
    float Vector3::angle(const Vector3& v) const
    {
        float angle = dot(v)/(magnitude()*v.magnitude());

        return acos(angle);
    }
Esempio n. 5
0
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;
}
Esempio n. 6
0
//----------------------------------------------------------------------------
Vector3 Curve3::GetTangent (Real fTime) const
{
    Vector3 kVelocity = GetFirstDerivative(fTime);
    kVelocity.Unitize();
    return kVelocity;
}
Esempio n. 7
0
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;
}
Esempio n. 8
0
bool XMLElement::SetVector3(const String& name, const Vector3& value)
{
    return SetAttribute(name, value.ToString());
}
Esempio n. 9
0
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;
}
Esempio n. 10
0
    //-----------------------------------------------------------------------
    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
    }
Esempio n. 11
0
//----------------------------------------------------------------------------------------------------
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;
}
Esempio n. 12
0
    //-----------------------------------------------------------------------
    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 );
        }
    }
Esempio n. 13
0
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 
  }
Esempio n. 15
0
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
}
Esempio n. 17
0
//----------------------------------------------------------------------------
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);
	}
}
Esempio n. 19
0
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;
}
Esempio n. 20
0
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;
}
Esempio n. 21
0
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;
}
Esempio n. 22
0
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);
}
Esempio n. 23
0
GeometryT<T> GeometryT<T>::translate(Vector3<T> v) {
  Vector3<T> withScale = v.applyScale();
  return this->translate(withScale.getX(), withScale.getY());
}
Esempio n. 24
0
// 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;
}
Esempio n. 26
0
void HalfSpace3<Real>::SetNormal (const Vector3<Real>& rkNormal)
{
    m_afTuple[0] = rkNormal.X();
    m_afTuple[1] = rkNormal.Y();
    m_afTuple[2] = rkNormal.Z();
}
Esempio n. 27
0
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();
}
Esempio n. 28
0
  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();
    }

  }
Esempio n. 29
0
// @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();
}