void MobileVRInterface::set_position_from_sensors() {
	_THREAD_SAFE_METHOD_

	// this is a helper function that attempts to adjust our transform using our 9dof sensors
	// 9dof is a misleading marketing term coming from 3 accelerometer axis + 3 gyro axis + 3 magnetometer axis = 9 axis
	// but in reality this only offers 3 dof (yaw, pitch, roll) orientation

	uint64_t ticks = OS::get_singleton()->get_ticks_usec();
	uint64_t ticks_elapsed = ticks - last_ticks;
	float delta_time = (double)ticks_elapsed / 1000000.0;

	// few things we need
	Input *input = Input::get_singleton();
	Vector3 down(0.0, -1.0, 0.0); // Down is Y negative
	Vector3 north(0.0, 0.0, 1.0); // North is Z positive

	// make copies of our inputs
	bool has_grav = false;
	Vector3 acc = input->get_accelerometer();
	Vector3 gyro = input->get_gyroscope();
	Vector3 grav = input->get_gravity();
	Vector3 magneto = scale_magneto(input->get_magnetometer()); // this may be overkill on iOS because we're already getting a calibrated magnetometer reading

	if (sensor_first) {
		sensor_first = false;
	} else {
		acc = scrub(acc, last_accerometer_data, 2, 0.2);
		magneto = scrub(magneto, last_magnetometer_data, 3, 0.3);
	};

	last_accerometer_data = acc;
	last_magnetometer_data = magneto;

	if (grav.length() < 0.1) {
		// not ideal but use our accelerometer, this will contain shakey shakey user behaviour
		// maybe look into some math but I'm guessing that if this isn't available, its because we lack the gyro sensor to actually work out
		// what a stable gravity vector is
		grav = acc;
		if (grav.length() > 0.1) {
			has_grav = true;
		};
	} else {
		has_grav = true;
	};

	bool has_magneto = magneto.length() > 0.1;
	if (gyro.length() > 0.1) {
		/* this can return to 0.0 if the user doesn't move the phone, so once on, it's on */
		has_gyro = true;
	};

	if (has_gyro) {
		// start with applying our gyro (do NOT smooth our gyro!)
		Basis rotate;
		rotate.rotate(orientation.get_axis(0), gyro.x * delta_time);
		rotate.rotate(orientation.get_axis(1), gyro.y * delta_time);
		rotate.rotate(orientation.get_axis(2), gyro.z * delta_time);
		orientation = rotate * orientation;

		tracking_state = ARVRInterface::ARVR_NORMAL_TRACKING;
	};

	///@TODO improve this, the magnetometer is very fidgity sometimes flipping the axis for no apparent reason (probably a bug on my part)
	// if you have a gyro + accelerometer that combo tends to be better then combining all three but without a gyro you need the magnetometer..
	if (has_magneto && has_grav && !has_gyro) {
		// convert to quaternions, easier to smooth those out
		Quat transform_quat(orientation);
		Quat acc_mag_quat(combine_acc_mag(grav, magneto));
		transform_quat = transform_quat.slerp(acc_mag_quat, 0.1);
		orientation = Basis(transform_quat);

		tracking_state = ARVRInterface::ARVR_NORMAL_TRACKING;
	} else if (has_grav) {
		// use gravity vector to make sure down is down...
		// transform gravity into our world space
		grav.normalize();
		Vector3 grav_adj = orientation.xform(grav);
		float dot = grav_adj.dot(down);
		if ((dot > -1.0) && (dot < 1.0)) {
			// axis around which we have this rotation
			Vector3 axis = grav_adj.cross(down);
			axis.normalize();

			Basis drift_compensation(axis, acos(dot) * delta_time * 10);
			orientation = drift_compensation * orientation;
		};
	};

	// JIC
	orientation.orthonormalize();

	last_ticks = ticks;
};

 Vector3 operator*(const Matrix4 &lhs, const Vector3 &rhs)
 {
    return Vector3(rhs.Transform(lhs));
 }

bool Cube::setLimits(const Molecule &mol, double spacing_, double padding)
{
  Index numAtoms = mol.atomCount();
  Vector3 min_, max_;
  if (numAtoms) {
    Vector3 curPos = min_ = max_ = mol.atomPositions3d()[0];
    for (Index i = 1; i < numAtoms; ++i) {
      curPos = mol.atomPositions3d()[i];
      if (curPos.x() < min_.x())
        min_.x() = curPos.x();
      if (curPos.x() > max_.x())
        max_.x() = curPos.x();
      if (curPos.y() < min_.y())
        min_.y() = curPos.y();
      if (curPos.y() > max_.y())
        max_.y() = curPos.y();
      if (curPos.z() < min_.z())
        min_.z() = curPos.z();
      if (curPos.z() > max_.z())
        max_.z() = curPos.z();
    }
  }
  else {
    min_ = max_ = Vector3::Zero();
  }

  // Now to take care of the padding term
  min_ += Vector3(-padding,-padding,-padding);
  max_ += Vector3( padding, padding, padding);

  return setLimits(min_, max_, spacing_);
}

    float Vector3::angle(const Vector3& v) const
    {
        float angle = dot(v)/(magnitude()*v.magnitude());

        return acos(angle);
    }

bool Wml::FindIntersection (const Sphere3<Real>& rkS0,
    const Sphere3<Real>& rkS1, Real fTime, const Vector3<Real>& rkV0,
    const Vector3<Real>& rkV1, Real& rfFirstTime,
    Vector3<Real>& rkFirstPoint)
{
    Vector3<Real> kVDiff = rkV1 - rkV0;
    Real fA = kVDiff.SquaredLength();
    Vector3<Real> kCDiff = rkS1.Center() - rkS0.Center();
    Real fC = kCDiff.SquaredLength();
    Real fRSum = rkS0.Radius() + rkS1.Radius();
    Real fRSumSqr = fRSum*fRSum;

    if ( fA > (Real)0.0 )
    {
        Real fB = kCDiff.Dot(kVDiff);
        if ( fB <= (Real)0.0 )
        {
            if ( -fTime*fA <= fB
            ||   fTime*(fTime*fA + ((Real)2.0)*fB) + fC <= fRSumSqr )
            {
                Real fCDiff = fC - fRSumSqr;
                Real fDiscr = fB*fB - fA*fCDiff;
                if ( fDiscr >= (Real)0.0 )
                {
                    if ( fCDiff <= (Real)0.0 )
                    {
                        // The spheres are initially intersecting.  Estimate a
                        // point of contact by using the midpoint of the line
                        // segment connecting the sphere centers.
                        rfFirstTime = (Real)0.0;
                        rkFirstPoint = ((Real)0.5)*(rkS0.Center() +
                            rkS1.Center());
                    }
                    else
                    {
                        // The first time of contact is in [0,fTime].
                        rfFirstTime = -(fB + Math<Real>::Sqrt(fDiscr))/fA;
                        if ( rfFirstTime < (Real)0.0 )
                            rfFirstTime = (Real)0.0;
                        else if ( rfFirstTime > fTime )
                            rfFirstTime = fTime;

                        Vector3<Real> kNewCDiff = kCDiff + rfFirstTime*kVDiff;

                        rkFirstPoint = rkS0.Center() + rfFirstTime*rkV0 +
                            (rkS0.Radius()/fRSum)*kNewCDiff;
                    }
                    return true;
                }
            }
            return false;
        }
    }

    if ( fC <= fRSumSqr )
    {
        // The spheres are initially intersecting.  Estimate a point of
        // contact by using the midpoint of the line segment connecting the
        // sphere centers.
        rfFirstTime = (Real)0.0;
        rkFirstPoint = ((Real)0.5)*(rkS0.Center() + rkS1.Center());
        return true;
    }

    return false;
}

//----------------------------------------------------------------------------
Vector3 Curve3::GetTangent (Real fTime) const
{
    Vector3 kVelocity = GetFirstDerivative(fTime);
    kVelocity.Unitize();
    return kVelocity;
}

bool SpaceSW::test_body_motion(BodySW *p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer::MotionResult *r_result) {

	//give me back regular physics engine logic
	//this is madness
	//and most people using this function will think
	//what it does is simpler than using physics
	//this took about a week to get right..
	//but is it right? who knows at this point..

	if (r_result) {
		r_result->collider_id = 0;
		r_result->collider_shape = 0;
	}
	AABB body_aabb;

	for (int i = 0; i < p_body->get_shape_count(); i++) {

		if (i == 0)
			body_aabb = p_body->get_shape_aabb(i);
		else
			body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
	}

	// Undo the currently transform the physics server is aware of and apply the provided one
	body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
	body_aabb = body_aabb.grow(p_margin);

	Transform body_transform = p_from;

	{
		//STEP 1, FREE BODY IF STUCK

		const int max_results = 32;
		int recover_attempts = 4;
		Vector3 sr[max_results * 2];

		do {

			PhysicsServerSW::CollCbkData cbk;
			cbk.max = max_results;
			cbk.amount = 0;
			cbk.ptr = sr;

			PhysicsServerSW::CollCbkData *cbkptr = &cbk;
			CollisionSolverSW::CallbackResult cbkres = PhysicsServerSW::_shape_col_cbk;

			bool collided = false;

			int amount = _cull_aabb_for_body(p_body, body_aabb);

			for (int j = 0; j < p_body->get_shape_count(); j++) {
				if (p_body->is_shape_set_as_disabled(j))
					continue;

				Transform body_shape_xform = body_transform * p_body->get_shape_transform(j);
				ShapeSW *body_shape = p_body->get_shape(j);
				for (int i = 0; i < amount; i++) {

					const CollisionObjectSW *col_obj = intersection_query_results[i];
					int shape_idx = intersection_query_subindex_results[i];

					if (CollisionSolverSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, NULL, p_margin)) {
						collided = cbk.amount > 0;
					}
				}
			}

			if (!collided) {
				break;
			}

			Vector3 recover_motion;

			for (int i = 0; i < cbk.amount; i++) {

				Vector3 a = sr[i * 2 + 0];
				Vector3 b = sr[i * 2 + 1];
				recover_motion += (b - a) * 0.4;
			}

			if (recover_motion == Vector3()) {
				collided = false;
				break;
			}

			body_transform.origin += recover_motion;
			body_aabb.position += recover_motion;

			recover_attempts--;

		} while (recover_attempts);
	}

	real_t safe = 1.0;
	real_t unsafe = 1.0;
	int best_shape = -1;

	{
		// STEP 2 ATTEMPT MOTION

//.........这里部分代码省略.........

bool XMLElement::SetVector3(const String& name, const Vector3& value)
{
    return SetAttribute(name, value.ToString());
}

bool ParticleEmitter::EmitNewParticle()
{
    unsigned index = GetFreeParticle();
    if (index == M_MAX_UNSIGNED)
        return false;
    assert(index < particles_.Size());
    Particle& particle = particles_[index];
    Billboard& billboard = billboards_[index];

    Vector3 startDir;
    Vector3 startPos;

    startDir = effect_->GetRandomDirection();
    startDir.Normalize();

    switch (effect_->GetEmitterType())
    {
    case EMITTER_SPHERE:
        {
            Vector3 dir(
                Random(2.0f) - 1.0f,
                Random(2.0f) - 1.0f,
                Random(2.0f) - 1.0f
            );
            dir.Normalize();
            startPos = effect_->GetEmitterSize() * dir * 0.5f;
        }
        break;

    case EMITTER_BOX:
        {
            const Vector3& emitterSize = effect_->GetEmitterSize();
            startPos = Vector3(
                Random(emitterSize.x_) - emitterSize.x_ * 0.5f,
                Random(emitterSize.y_) - emitterSize.y_ * 0.5f,
                Random(emitterSize.z_) - emitterSize.z_ * 0.5f
            );
        }
        break;
    }

    particle.size_ = effect_->GetRandomSize();
    particle.timer_ = 0.0f;
    particle.timeToLive_ = effect_->GetRandomTimeToLive();
    particle.scale_ = 1.0f;
    particle.rotationSpeed_ = effect_->GetRandomRotationSpeed();
    particle.colorIndex_ = 0;
    particle.texIndex_ = 0;

    if (faceCameraMode_ == FC_DIRECTION)
    {
        startPos += startDir * particle.size_.y_;
    }

    if (!relative_)
    {
        startPos = node_->GetWorldTransform() * startPos;
        startDir = node_->GetWorldRotation() * startDir;
    };

    particle.velocity_ = effect_->GetRandomVelocity() * startDir;

    billboard.position_ = startPos;
    billboard.size_ = particles_[index].size_;
    const Vector<TextureFrame>& textureFrames_ = effect_->GetTextureFrames();
    billboard.uv_ = textureFrames_.Size() ? textureFrames_[0].uv_ : Rect::POSITIVE;
    billboard.rotation_ = effect_->GetRandomRotation();
    const Vector<ColorFrame>& colorFrames_ = effect_->GetColorFrames();
    billboard.color_ = colorFrames_.Size() ? colorFrames_[0].color_ : Color();
    billboard.enabled_ = true;
    billboard.direction_ = startDir;

    return true;
}

    //-----------------------------------------------------------------------
    void ConvexBody::clip( const Plane& pl, bool keepNegative )
    {
        if ( getPolygonCount() == 0 )
            return;

        // current will be used as the reference body
        ConvexBody current;
        current.moveDataFromBody(*this);
        
        OgreAssert( this->getPolygonCount() == 0, "Body not empty!" );
        OgreAssert( current.getPolygonCount() != 0, "Body empty!" );

        // holds all intersection edges for the different polygons
        Polygon::EdgeMap intersectionEdges;

        // clip all polygons by the intersection plane
        // add only valid or intersected polygons to *this
        for ( size_t iPoly = 0; iPoly < current.getPolygonCount(); ++iPoly )
        {

            // fetch vertex count and ignore polygons with less than three vertices
            // the polygon is not valid and won't be added
            const size_t vertexCount = current.getVertexCount( iPoly );
            if ( vertexCount < 3 )
                continue;

            // current polygon
            const Polygon& p = current.getPolygon( iPoly );

            // the polygon to assemble
            Polygon *pNew = allocatePolygon();

            // the intersection polygon (indeed it's an edge or it's empty)
            Polygon *pIntersect = allocatePolygon();
            
            // check if polygons lie inside or outside (or on the plane)
            // for each vertex check where it is situated in regard to the plane
            // three possibilities appear:
            Plane::Side clipSide = keepNegative ? Plane::POSITIVE_SIDE : Plane::NEGATIVE_SIDE;
            // - side is clipSide: vertex will be clipped
            // - side is !clipSide: vertex will be untouched
            // - side is NOSIDE:   vertex will be untouched
            Plane::Side *side = OGRE_ALLOC_T(Plane::Side, vertexCount, MEMCATEGORY_SCENE_CONTROL);
            for ( size_t iVertex = 0; iVertex < vertexCount; ++iVertex )
            {
                side[ iVertex ] = pl.getSide( p.getVertex( iVertex ) );
            }

            // now we check the side combinations for the current and the next vertex
            // four different combinations exist:
            // - both points inside (or on the plane): keep the second (add it to the body)
            // - both points outside: discard both (don't add them to the body)
            // - first vertex is inside, second is outside: add the intersection point
            // - first vertex is outside, second is inside: add the intersection point, then the second
            for ( size_t iVertex = 0; iVertex < vertexCount; ++iVertex )
            {
                // determine the next vertex
                size_t iNextVertex = ( iVertex + 1 ) % vertexCount;

                const Vector3& vCurrent = p.getVertex( iVertex );
                const Vector3& vNext    = p.getVertex( iNextVertex );

                // case 1: both points inside (store next)
                if ( side[ iVertex ]     != clipSide &&     // NEGATIVE or NONE
                     side[ iNextVertex ] != clipSide )      // NEGATIVE or NONE
                {
                    // keep the second
                    pNew->insertVertex( vNext );
                }

                // case 3: inside -> outside (store intersection)
                else if ( side[ iVertex ]       != clipSide &&
                          side[ iNextVertex ]   == clipSide )
                {
                    // Do an intersection with the plane. We use a ray with a start point and a direction.
                    // The ray is forced to hit the plane with any option available (eigher current or next
                    // is the starting point)

                    // intersect from the outside vertex towards the inside one
                    Vector3 vDirection = vCurrent - vNext;
                    vDirection.normalise();
                    Ray ray( vNext, vDirection );
                    std::pair< bool, Real > intersect = ray.intersects( pl );

                    // store intersection
                    if ( intersect.first )
                    {
                        // convert distance to vector
                        Vector3 vIntersect = ray.getPoint( intersect.second );  

                        // store intersection
                        pNew->insertVertex( vIntersect );
                        pIntersect->insertVertex( vIntersect );
                    }
                }

                // case 4: outside -> inside (store intersection, store next)
                else if ( side[ iVertex ]       == clipSide &&
                    side[ iNextVertex ]         != clipSide )
//.........这里部分代码省略.........

//----------------------------------------------------------------------------------------------------
Vector3 Scene::getColor (Ray r, double intensity, int depth, double tolerance)
{
    Vector3 ret;
    Vector3 color;
    Object *obj;
    Vector3 normal;
    Vector3 intersection;

    int objIndex;
    
    Vector3 diffuse, phong;
    Ray lightR;
    
    double d = getIntersection(&objIndex, r);
    
    // Ray has no intersection
    if (d <= 0.0)
        return Vector3(0.0, 0.0, 0.0);

    // Get intersection point
    intersection = r.move(d);
    
    obj = objects[objIndex];
    color = obj->getColor();
    normal = obj->getNormal(intersection);

    // Ambient color
    ret = 0.15 * color;

    // For each light
    int n = lights.size();
    for (int i=0; i<n; i++)
    {
        lightR = Ray( intersection, lights[i].position - intersection );
        
        // Verify if object is not in the shadow
        if ( !hasIntersection(lightR, lights[i].position) )
        {
            Vector3 lightColor = lights[i].getColor();
            
            // Calculate diffuse illumination
            diffuse = color * max(normal * lightR.direction, 0.0);
            diffuse = Vector3(lightColor.x * diffuse.x, lightColor.y * diffuse.y, lightColor.z * diffuse.z);
            
            // Calculate specular illumination
            Vector3 h = (lights[i].position - intersection) + (camera.position - intersection);
            h.normalize();
            phong += lightColor * pow(max(h * normal, 0.0), 128);
            
            ret += diffuse;
        }
    }

    
    Vector3 refColor;
    double reflect = obj->reflect;
    
    // Test base cases for recursion
    if ((depth > 1) && (intensity*reflect > tolerance))
    {
        Ray refRay(intersection, r.direction - 2*(r.direction * normal) * normal);

        refColor = getColor(refRay, reflect*intensity, --depth, tolerance);
    }
    
    // Sum all color and check for overflows
    ret = intensity * ( (1 - reflect) * (0.9 * ret) + reflect * refColor + phong);
    
    if (ret.x > 1)
        ret.x = 1;
    if (ret.y > 1)
        ret.y = 1;
    if (ret.z > 1)
        ret.z = 1;
    
    return ret;
}

    //-----------------------------------------------------------------------
    void ConvexBody::extend(const Vector3& pt)
    {
        // Erase all polygons facing towards the point. For all edges that
        // are not removed twice (once in AB and once BA direction) build a
        // convex polygon (triangle) with the point.
        Polygon::EdgeMap edgeMap;

        for ( size_t i = 0; i < getPolygonCount(); ++i )
        {
            const Vector3& normal = getNormal( i );
            // direction of the point in regard to the polygon
            // the polygon is planar so we can take an arbitrary vertex
            Vector3 ptDir  = pt - getVertex( i, 0 );
            ptDir.normalise();

            // remove polygon if dot product is greater or equals null.
            if ( normal.dotProduct( ptDir ) >= 0 )
            {
                // store edges (copy them because if the polygon is deleted
                // its vertices are also deleted)
                storeEdgesOfPolygon( i, &edgeMap );

                // remove polygon
                deletePolygon( i );

                // decrement iterator because of deleted polygon
                --i; 
            }
        }

        // point is already a part of the hull (point lies inside)
        if ( edgeMap.empty() )
            return;

        // remove the edges that are twice in the list (once from each side: AB,BA)

        Polygon::EdgeMap::iterator it;
        // iterate from first to the element before the last one
        for (Polygon::EdgeMap::iterator itStart = edgeMap.begin(); 
            itStart != edgeMap.end(); )
        {
            // compare with iterator + 1 to end
            // don't need to skip last entry in itStart since omitted in inner loop
            it = itStart;
            ++it;

            bool erased = false;
            // iterate from itStart+1 to the element before the last one
            for ( ; it != edgeMap.end(); ++it )
            {   
                if (itStart->first.positionEquals(it->second) &&
                     itStart->second.positionEquals(it->first))
                {
                    edgeMap.erase(it);
                    // increment itStart before deletion (iterator invalidation)
                    Polygon::EdgeMap::iterator delistart = itStart++;
                    edgeMap.erase(delistart);
                    erased = true;

                    break; // found and erased
                }
            }
            // increment itStart if we didn't do it when erasing
            if (!erased)
                ++itStart;

        }

        // use the remaining edges to build triangles with the point
        // the vertices of the edges are in ccw order (edgePtA-edgePtB-point
        // to form a ccw polygon)
        while ( !edgeMap.empty() )
        {
            Polygon::EdgeMap::iterator mapIt = edgeMap.begin();

            // build polygon it.first, it.second, point
            Polygon *p = allocatePolygon();

            p->insertVertex(mapIt->first);
            p->insertVertex(mapIt->second);

            p->insertVertex( pt );
            // attach polygon to body
            insertPolygon( p );

            // erase the vertices from the list
            // pointers are now held by the polygon
            edgeMap.erase( mapIt );
        }
    }

Vector3 operator*(const float &a, const Vector3 &vec) {
  Vector3 ret(a*vec.x(), a*vec.y(), a*vec.z());
  return ret;
}

  void createTerrainLightmap(const TerrainInfo& info, Image& image,
    size_t width, size_t height, Vector3 lightDir, const ColourValue& lightCol,
    const ColourValue& ambient, bool shadowed)
  {
    lightDir.normalise();

    // calculate lightmap by multiplying light dir with terrain normals

    // calculate the step size to use
    AxisAlignedBox extents = info.getExtents();
    Vector3 startPos = extents.getMinimum();
    Vector3 step = extents.getMaximum() - extents.getMinimum();
    step.x /= width;
    step.z /= height;
    Vector3 pos = startPos;

#if OGRE_VERSION_MINOR > 4
    // Ogre::Image uses the memory allocation macros internally in Shoggoth,
    // so we must use them as well.
    uchar* lightMap = OGRE_ALLOC_T(uchar, width*height*3, MEMCATEGORY_GENERAL);
#else
    uchar* lightMap = new uchar[width*height * 3];
#endif
    memset(lightMap, 255, width*height*3);

    for (size_t z = 0; z < height; ++z)
    {
      for (size_t x = 0; x < width; ++x)
      {
        size_t index = (z * width + x)*3;
        // calculate diffuse light from light source
        Vector3 norm = info.getNormalAt(pos.x, pos.z);
        float l = std::max(0.0f, -lightDir.dotProduct(norm));

        ColourValue v = ambient;
        v.r = std::min(1.0f, v.r+l*lightCol.r);
        v.g = std::min(1.0f, v.g+l*lightCol.g);
        v.b = std::min(1.0f, v.b+l*lightCol.b);
        lightMap[index+0] = (uchar) (255*v.r);
        lightMap[index+1] = (uchar) (255*v.g);
        lightMap[index+2] = (uchar) (255*v.b);

        pos.x += step.x;
      }
      pos.x = startPos.x;
      pos.z += step.z;
    }

    if (shadowed && (lightDir.x != 0 || lightDir.z != 0))
    {
      // add terrain shadows
      Impl::addTerrainShadowsToLightmap(lightMap, info, width, height, lightDir, ambient);
    }

    // use a box filter to smoothen the lightmap
    Impl::boxFilterLightmap(lightMap, width, height);

    // save lightmap to image
    image.loadDynamicImage(lightMap, width, height, 1, PF_BYTE_RGB, true);
    // ownership of lightMap was transfered to image, don't need to delete 
  }

void AILookStrategy::Step(unsigned timeMs)
{		
    IPhysical *phys = Parent->GetPhysical();
    IScenable *scen = Parent->GetScenable();
	
	if(RotationUnit.mRotating)
	{
		RotationUnit.Step();
		if(RotationUnit.mRotating)                                // Process timed rotation
		{				
			Ogre::Quaternion delta = RotationUnit.Slerp();	
			scen->SetOrientation(delta);
		}
	} else
	{
		Ogre::Quaternion OurOrientation = scen->GetOrientation();
		
		//Ogre::Vector3 src = OurOrientation * Ogre::Vector3::NEGATIVE_UNIT_Z;
		Ogre::Vector3 direction = Ogre::Vector3::ZERO;
		switch(LookType) {
		case LT_FORWARD:
			direction = -phys->GetForwardDirection();
			break;
		case LT_DIRECTION:			
			direction = Direction;
			break;
		case LT_OBJECT:
			{
				if (TargetID<0)
				{
					assert(false);					
				}

				if (TargetID==0)
				{
					if (Owner)
					{
						TargetID = Owner->SelectTargetID();			
					}					
				}

				IAAObject *obj = CommonDeclarations::GetIDObject(TargetID);
				//assert(obj);
				if (obj)
					direction = obj->GetPosition()-scen->GetPosition();			
				break;
			}
		};
				
		if (!direction.isZeroLength())
		{
			Vector3 xVec = Ogre::Vector3::UNIT_Y.crossProduct(direction);
			xVec.normalise();
			Vector3 yVec = direction.crossProduct(xVec);
			yVec.normalise();
			Quaternion unitZToTarget = Quaternion(xVec, yVec, direction);

			Quaternion targetOrientation = Quaternion(-unitZToTarget.y, -unitZToTarget.z, unitZToTarget.w, unitZToTarget.x);

			RotationUnit.StartRotation(OurOrientation, targetOrientation);
		}		

	}
}

double SteeringBehaviour :: getIntersectionTime (const Vector3& agent_position,
                                                 double agent_speed,
                                                 const Vector3& target_position,
                                                 const Vector3& target_velocity)
{
	assert(agent_speed >= 0.0);

	if(agent_speed == 0.0)
	{
		if(DEBUGGING_INTERSECTION_TIME)
			cout << "No #1" << endl;
		return NO_INTERSECTION_POSSIBLE;  // can't catch if can't move
	}

	if(target_velocity.isZero())
	{
		if(DEBUGGING_INTERSECTION_TIME)
			cout << "No #2" << endl;
		return getIntersectionTime(agent_position, agent_speed, target_position);
	}

	Vector3   relative_position = target_position - agent_position;
	double  agent_speed_squared = agent_speed * agent_speed;
	double target_speed_squared = target_velocity.getNormSquared();
	double     distance_squared = relative_position.getNormSquared();

	if(agent_speed_squared == target_speed_squared)
	{
		//
		//  When the speeds are the same.  I don't know why case
		//    this corresponds to, but we don't want a
		//    divide-by-0 error.  Some of these should have
		//    solutions, and those will sadly be missed.
		//
		if(DEBUGGING_INTERSECTION_TIME)
			cout << "No #3" << endl;
		return NO_INTERSECTION_POSSIBLE;
	}

	double a = target_speed_squared - agent_speed_squared;
	double b = target_velocity.dotProduct(-relative_position) * -2.0;
	double c = distance_squared;
	if(DEBUGGING_INTERSECTION_TIME)
	{
		cout << "\tdistance: " << sqrt(distance_squared) << endl;
		cout << "\ta: " << a << "\t==> " << (a / target_speed_squared) << endl;
		cout << "\tb: " << b << "\t==> " << (b / target_speed_squared) << endl;
		cout << "\tc: " << c << "\t==> " << (c / target_speed_squared) << endl;
	}

	double discriminant = b * b - 4 * a * c;
	if(discriminant < 0.0)
	{
		if(DEBUGGING_INTERSECTION_TIME)
			cout << "No #4" << endl;
		return NO_INTERSECTION_POSSIBLE;  // target is too fast
	}

	assert(discriminant >= 0.0);
	double sqrt_discriminant = sqrt(discriminant);

	assert(a != 0.0);
	double time1 = (-b - sqrt_discriminant) / (a * 2.0);
	if(time1 >= 0.0)
	{
		if(DEBUGGING_INTERSECTION_TIME)
		{
			cout << "time1: "       <<  time1
			     << "  \tcurrent: " <<          TimeSystem::getFrameStartTime()
			     << "  \ttotal: "   << (time1 + TimeSystem::getFrameStartTime()) << endl;
		}
		return time1;  // first possible intersection
	}
	
	assert(a != 0.0);
	double time2 = (-b + sqrt_discriminant) / (a * 2.0);
	if(time2 >= 0.0)
	{
		if(DEBUGGING_INTERSECTION_TIME)
		{
			cout << "time2: "       <<  time2
			     << "  \tcurrent: " <<          TimeSystem::getFrameStartTime()
			     << "  \ttotal: "   << (time2 + TimeSystem::getFrameStartTime()) << endl;
		}
		return time2;  // second possible intersection
	}

	if(DEBUGGING_INTERSECTION_TIME)
		cout << "No #5" << endl;
	return NO_INTERSECTION_POSSIBLE;  // both intersections are in the past
}

// 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)
//.........这里部分代码省略.........

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();
//.........这里部分代码省略.........

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_NO