C++ Transform类代码示例

void ParticleSystemProcessSW::process(const ParticleSystemSW *p_system,const Transform& p_transform,float p_time) {

	valid=false;
	if (p_system->amount<=0) {
		ERR_EXPLAIN("Invalid amount of particles: "+itos(p_system->amount));
		ERR_FAIL_COND(p_system->amount<=0);
	}
	if (p_system->attractor_count<0 || p_system->attractor_count>VS::MAX_PARTICLE_ATTRACTORS) {
		ERR_EXPLAIN("Invalid amount of particle attractors.");
		ERR_FAIL_COND(p_system->attractor_count<0 || p_system->attractor_count>VS::MAX_PARTICLE_ATTRACTORS);
	}
	float lifetime = p_system->particle_vars[VS::PARTICLE_LIFETIME];
	if (lifetime<CMP_EPSILON) {
		ERR_EXPLAIN("Particle system lifetime too small.");
		ERR_FAIL_COND(lifetime<CMP_EPSILON);
	}
	valid=true;
	int particle_count=MIN(p_system->amount,ParticleSystemSW::MAX_PARTICLES);;


	int emission_point_count = p_system->emission_points.size();
	DVector<Vector3>::Read r;
	if (emission_point_count)
		r=p_system->emission_points.read();

	if (particle_count!=particle_data.size()) {

		//clear the whole system if particle amount changed
		particle_data.clear();
		particle_data.resize(p_system->amount);
		particle_system_time=0;
	}

	float next_time = particle_system_time+p_time;
	
	if (next_time > lifetime)
		next_time=Math::fmod(next_time,lifetime);
		

	ParticleData *pdata=&particle_data[0];
	Vector3 attractor_positions[VS::MAX_PARTICLE_ATTRACTORS];

	for(int i=0;i<p_system->attractor_count;i++) {

		attractor_positions[i]=p_transform.xform(p_system->attractors[i].pos);
	}


	for(int i=0;i<particle_count;i++) {
	
		ParticleData &p=pdata[i];
		
		float restart_time = (i * lifetime / p_system->amount);
		
		bool restart=false;
		
		if ( next_time < particle_system_time ) {

			if (restart_time > particle_system_time || restart_time < next_time )
				restart=true;

		} else if (restart_time > particle_system_time && restart_time < next_time ) {
			restart=true;
		}

		if (restart) {


			if (p_system->emitting) {
				if (emission_point_count==0) { //use AABB
					if (p_system->local_coordinates)
						p.pos = p_system->emission_half_extents * Vector3( _rand_from_seed(&rand_seed), _rand_from_seed(&rand_seed), _rand_from_seed(&rand_seed) );
					else
						p.pos = p_transform.xform( p_system->emission_half_extents * Vector3( _rand_from_seed(&rand_seed), _rand_from_seed(&rand_seed), _rand_from_seed(&rand_seed) ) );
				} else {
					//use preset positions
					if (p_system->local_coordinates)
						p.pos = r[_irand_from_seed(&rand_seed)%emission_point_count];
					else
						p.pos = p_transform.xform( r[_irand_from_seed(&rand_seed)%emission_point_count] );
				}
							
				
				float angle1 = _rand_from_seed(&rand_seed)*p_system->particle_vars[VS::PARTICLE_SPREAD]*Math_PI;
				float angle2 = _rand_from_seed(&rand_seed)*20.0*Math_PI; // make it more random like
				
				Vector3 rot_xz=Vector3( Math::sin(angle1), 0.0, Math::cos(angle1) );
				Vector3 rot = Vector3( Math::cos(angle2)*rot_xz.x,Math::sin(angle2)*rot_xz.x, rot_xz.z);

				p.vel=(rot*p_system->particle_vars[VS::PARTICLE_LINEAR_VELOCITY]+rot*p_system->particle_randomness[VS::PARTICLE_LINEAR_VELOCITY]*_rand_from_seed(&rand_seed));
				if (!p_system->local_coordinates)
					p.vel=p_transform.basis.xform( p.vel );

				p.vel+=p_system->emission_base_velocity;
				
				p.rot=p_system->particle_vars[VS::PARTICLE_INITIAL_ANGLE]+p_system->particle_randomness[VS::PARTICLE_INITIAL_ANGLE]*_rand_from_seed(&rand_seed);				
				p.active=true;
				for(int r=0;r<PARTICLE_RANDOM_NUMBERS;r++)
					p.random[r]=_rand_from_seed(&rand_seed);

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

void ClientApplication::initEntities()
{
	Entity* entity = NULL;
	Component* component = NULL;

	// Read from assemblage
	AssemblageHelper::E_FileStatus status = AssemblageHelper::FileStatus_OK;
	EntityFactory* factory = static_cast<EntityFactory*>
		( m_world->getSystem( SystemType::EntityFactory ) );

	status = factory->readAssemblageFile( "Assemblages/testSpotLight.asd" );

	EntitySystem* tempSys = NULL;

	tempSys = m_world->getSystem(SystemType::GraphicsBackendSystem);
	GraphicsBackendSystem* graphicsBackend = static_cast<GraphicsBackendSystem*>(tempSys);

	/************************************************************************/
	/* Create the main camera used to render the scene						*/
	/************************************************************************/
	entity = m_world->createEntity();
	entity->setName("MainCamera");
	entity->addComponent( new CameraInfo( m_world->getAspectRatio(),1.3f,1.0f,3000.0f ) );
	entity->addComponent( new MainCamera_TAG() );
	entity->addComponent( new AudioListener() );
	entity->addComponent( new Transform( 0.0f, 0.0f, 0.0f ) );
	m_world->addEntity(entity);


	/************************************************************************/
	/* Create shadow camera and spotlight.									*/
	/************************************************************************/
	float rotation = 0.78;
	AglQuaternion quat;
	for(int i = 0; i < 1; i++){
		entity = factory->entityFromRecipe( "SpotLight" );
		LightsComponent* lightComp = static_cast<LightsComponent*>(
			entity->getComponent(ComponentType::LightsComponent));
		int shadowIdx = -1;
		vector<Light>* lights = lightComp->getLightsPtr();

		for (unsigned int i = 0; i < lights->size(); i++){
			if(lights->at(i).instanceData.shadowIdx != -1){
				shadowIdx = graphicsBackend->getGfxWrapper()->generateShadowMap();
				lights->at(i).instanceData.shadowIdx = shadowIdx;
			}
		}

		Transform* transform = static_cast<Transform*>(
			entity->getComponent(ComponentType::Transform));

		quat = AglQuaternion::constructFromAxisAndAngle(AglVector3::up(),rotation);
		transform->setRotation(quat);

		CameraInfo* cameraInfo = new CameraInfo(1);
		cameraInfo->m_shadowMapIdx = shadowIdx;
		entity->addComponent(ComponentType::CameraInfo, cameraInfo);
		entity->addTag(ComponentType::TAG_ShadowCamera, new ShadowCamera_TAG());
		m_world->addEntity( entity );

		rotation -= 0.78;
	}
}

 Vector3 CameraComponent::GetRotation()
 {
   Transform* tr = static_cast<Transform*>(GetSibling(CT_Transform));
   return tr->GetRotation();
 }

int BasicSceneExample::execute()
{	
	roll = 0.0f;

	while (isOpen())
	{
		Vector<3, float> movement(0.0f, 0.0f, 0.0f);
		const float moveSpeed = 0.06f;
		
		if (moveForward->isTriggered())
			movement += camera.getLook() * moveSpeed;
		if (moveBack->isTriggered())
			movement -= camera.getLook() * moveSpeed;
		if (strafeLeft->isTriggered())
			movement -= camera.getRight() * moveSpeed;
		if (strafeRight->isTriggered())
			movement += camera.getRight() * moveSpeed;

		const float rollSpeed = 2.0f;

		if (rollCCW->isTriggered())
			roll += rollSpeed;

		if (rollCW->isTriggered())
			roll -= rollSpeed;

		if (object->getSkeleton() && object->getSkeleton()->isLoaded())
		{
			// Pose
			const Skeleton* skeleton = object->getSkeleton().get();

			for (std::size_t i = 0; i < skeleton->getBoneCount(); ++i)
			{
				pose->setRelativeTransform(i, skeleton->getBindPose()->getRelativeTransform(i));
			}

			Quaternion<float> leftFemurRotation, leftTibiaRotation, rightFemurRotation, rightTibiaRotation;
			leftFemurRotation = Quaternion<float>::fromAxisAngle({0.0f, 1.0f, 0.0f}, math::radians<float>(roll * 0.25f));
			leftTibiaRotation = Quaternion<float>::fromAxisAngle({0.0f, 1.0f, 0.0f}, math::radians<float>(roll));
			rightFemurRotation = Quaternion<float>::fromAxisAngle({0.0f, 0.0f, 1.0f}, math::radians<float>(45.0f));
			rightTibiaRotation = Quaternion<float>::fromAxisAngle({0.0f, 0.0f, 1.0f}, math::radians<float>(-75.0f));

			const Bone* bone = skeleton->getBone("anterior_abdomen");
			if (bone)
			{
				std::size_t index = bone->getIndex();
				
				Transform<float> transform = pose->getRelativeTransform(index);
				transform.setRotation(transform.getRotation() * leftTibiaRotation);
				pose->setRelativeTransform(index, transform);
			}

			bone = skeleton->getBone("posterior_abdomen");
			if (bone)
			{
				std::size_t index = bone->getIndex();
				
				Transform<float> transform = pose->getRelativeTransform(index);
				transform.setRotation(transform.getRotation() * leftFemurRotation);
				pose->setRelativeTransform(index, transform);
			}

			/*
			bone = skeleton->getBone("right_midleg_femur");
			if (bone)
			{
				std::size_t index = bone->getIndex();
				
				Transform<float> transform = pose->getRelativeTransform(index);
				transform.setRotation(transform.getRotation() * rightFemurRotation);
				pose->setRelativeTransform(index, transform);
			}

			bone = skeleton->getBone("right_midleg_tibia");
			if (bone)
			{
				std::size_t index = bone->getIndex();
				
				Transform<float> transform = pose->getRelativeTransform(index);
				transform.setRotation(transform.getRotation() * rightTibiaRotation);
				pose->setRelativeTransform(index, transform);
			}
			*/

			pose->concatenate();
		}


			if (0 && roll != 0.0f)
			{
				Quaternion<float> rotation = Quaternion<float>::fromAxisAngle(camera.getLook(), roll).normalized();

				up = rotation * up;

				updateCamera();
			}

			camera.setAspectRatio(window->getViewport().getAspectRatio());
			camera.lookAt(
				camera.getPosition() + movement,
//.........这里部分代码省略.........

Vector<Plane> CameraMatrix::get_projection_planes(const Transform& p_transform) const {

	/** Fast Plane Extraction from combined modelview/projection matrices.
	 * References:
	 * http://www.markmorley.com/opengl/frustumculling.html
	 * http://www2.ravensoft.com/users/ggribb/plane%20extraction.pdf
	 */

	Vector<Plane> planes;

	const float * matrix = (const float*)this->matrix;

	Plane new_plane;

	///////--- Near Plane ---///////
	new_plane=Plane(matrix[ 3] + matrix[ 2],
		      matrix[ 7] + matrix[ 6],
		      matrix[11] + matrix[10],
		      matrix[15] + matrix[14]);

	new_plane.normal=-new_plane.normal;
	new_plane.normalize();

	planes.push_back( p_transform.xform(new_plane) );

	///////--- Far Plane ---///////
	new_plane=Plane(matrix[ 3] - matrix[ 2],
		      matrix[ 7] - matrix[ 6],
		      matrix[11] - matrix[10],
		      matrix[15] - matrix[14]);

	new_plane.normal=-new_plane.normal;
	new_plane.normalize();

	planes.push_back( p_transform.xform(new_plane) );


	///////--- Left Plane ---///////
	new_plane=Plane(matrix[ 3] + matrix[ 0],
		      matrix[ 7] + matrix[ 4],
		      matrix[11] + matrix[ 8],
		      matrix[15] + matrix[12]);

	new_plane.normal=-new_plane.normal;
	new_plane.normalize();

	planes.push_back( p_transform.xform(new_plane) );


	///////--- Top Plane ---///////
	new_plane=Plane(matrix[ 3] - matrix[ 1],
		      matrix[ 7] - matrix[ 5],
		      matrix[11] - matrix[ 9],
		      matrix[15] - matrix[13]);


	new_plane.normal=-new_plane.normal;
	new_plane.normalize();

	planes.push_back( p_transform.xform(new_plane) );


	///////--- Right Plane ---///////
	new_plane=Plane(matrix[ 3] - matrix[ 0],
		      matrix[ 7] - matrix[ 4],
		      matrix[11] - matrix[ 8],
		      matrix[15] - matrix[12]);


	new_plane.normal=-new_plane.normal;
	new_plane.normalize();

	planes.push_back( p_transform.xform(new_plane) );


	///////--- Bottom Plane ---///////
	new_plane=Plane(matrix[ 3] + matrix[ 1],
		      matrix[ 7] + matrix[ 5],
		      matrix[11] + matrix[ 9],
		      matrix[15] + matrix[13]);


	new_plane.normal=-new_plane.normal;
	new_plane.normalize();

	planes.push_back( p_transform.xform(new_plane) );

	return planes;
}

void DefaultGeometry::generateDecorations
   (const State&                         state, 
    Array_<SimTK::DecorativeGeometry>&   geometry) 
{
    const SimbodyMatterSubsystem& matter = _model.getMatterSubsystem();
    const ModelDisplayHints&      hints  = _model.getDisplayHints();


    // Display wrap objects.
    if (hints.get_show_wrap_geometry()) {
        const Vec3 color(SimTK::Cyan);
        Transform ztoy;
        ztoy.updR().setRotationFromAngleAboutX(SimTK_PI/2);
        const BodySet& bodies = _model.getBodySet();
        for (int i = 0; i < bodies.getSize(); i++) {
            const OpenSim::Body& body = bodies[i];
            const Transform& X_GB =
                body.getMobilizedBody().getBodyTransform(state);
            const WrapObjectSet& wrapObjects = body.getWrapObjectSet();
            for (int j = 0; j < wrapObjects.getSize(); j++) {
                const string type = wrapObjects[j].getConcreteClassName();
                if (type == "WrapCylinder") {
                    const WrapCylinder* cylinder = 
                        dynamic_cast<const WrapCylinder*>(&wrapObjects[j]);
                    if (cylinder != NULL) {
                        Transform X_GW = X_GB*cylinder->getTransform()*ztoy;
                        geometry.push_back(
                            DecorativeCylinder(cylinder->getRadius(), 
                                               cylinder->getLength()/2)
                                .setTransform(X_GW).setResolution(_dispWrapResolution)
                                .setColor(color).setOpacity(_dispWrapOpacity));
                    }
                }
                else if (type == "WrapEllipsoid") {
                    const WrapEllipsoid* ellipsoid = 
                        dynamic_cast<const WrapEllipsoid*>(&wrapObjects[j]);
                    if (ellipsoid != NULL) {
                        Transform X_GW = X_GB*ellipsoid->getTransform();
                        geometry.push_back(
                            DecorativeEllipsoid(ellipsoid->getRadii())
                                .setTransform(X_GW).setResolution(_dispWrapResolution)
                                .setColor(color).setOpacity(_dispWrapOpacity));
                    }
                }
                else if (type == "WrapSphere") {
                    const WrapSphere* sphere = 
                        dynamic_cast<const WrapSphere*>(&wrapObjects[j]);
                    if (sphere != NULL) {
                        Transform X_GW = X_GB*sphere->getTransform();
                        geometry.push_back(
                            DecorativeSphere(sphere->getRadius())
                                .setTransform(X_GW).setResolution(_dispWrapResolution)
                                .setColor(color).setOpacity(_dispWrapOpacity));
                    }
                }
            }
        }
    }


    // Display contact geometry objects.
    if (hints.get_show_contact_geometry()) {
        const Vec3 color(SimTK::Green);
        Transform ztoy;
        ztoy.updR().setRotationFromAngleAboutX(SimTK_PI/2);
        const ContactGeometrySet& contactGeometries = _model.getContactGeometrySet();

        for (int i = 0; i < contactGeometries.getSize(); i++) {
            const PhysicalFrame& body = contactGeometries.get(i).getBody();
            const Transform& X_GB = 
                matter.getMobilizedBody(body.getMobilizedBodyIndex()).getBodyTransform(state);
            const string type = contactGeometries.get(i).getConcreteClassName();
            const int displayPref = contactGeometries.get(i).getDisplayPreference();
            //cout << type << ": " << contactGeometries.get(i).getName() << ": disp pref = " << displayPref << endl;

            if (type == "ContactSphere" && displayPref == 4) {
                ContactSphere* sphere = 
                    dynamic_cast<ContactSphere*>(&contactGeometries.get(i));
                if (sphere != NULL) {
                    Transform X_GW = X_GB*sphere->getTransform();
                    geometry.push_back(
                        DecorativeSphere(sphere->getRadius())
                            .setTransform(X_GW).setResolution(_dispContactResolution)
                            .setColor(color).setOpacity(_dispContactOpacity));
                }
            }
        }
    }


    // Ask all the ModelComponents to generate dynamic geometry.
    _model.generateDecorations(false, _model.getDisplayHints(),
                               state, geometry);
}

void BodySW::integrate_velocities(real_t p_step) {

	if (mode == PhysicsServer::BODY_MODE_STATIC)
		return;

	if (fi_callback)
		get_space()->body_add_to_state_query_list(&direct_state_query_list);

	if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {

		_set_transform(new_transform, false);
		_set_inv_transform(new_transform.affine_inverse());
		if (contacts.size() == 0 && linear_velocity == Vector3() && angular_velocity == Vector3())
			set_active(false); //stopped moving, deactivate

		return;
	}

	//apply axis lock
	if (axis_lock != PhysicsServer::BODY_AXIS_LOCK_DISABLED) {

		int axis = axis_lock - 1;
		for (int i = 0; i < 3; i++) {
			if (i == axis) {
				linear_velocity[i] = 0;
				biased_linear_velocity[i] = 0;
			} else {

				angular_velocity[i] = 0;
				biased_angular_velocity[i] = 0;
			}
		}
	}

	Vector3 total_angular_velocity = angular_velocity + biased_angular_velocity;

	real_t ang_vel = total_angular_velocity.length();
	Transform transform = get_transform();

	if (ang_vel != 0.0) {
		Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
		Basis rot(ang_vel_axis, ang_vel * p_step);
		Basis identity3(1, 0, 0, 0, 1, 0, 0, 0, 1);
		transform.origin += ((identity3 - rot) * transform.basis).xform(center_of_mass_local);
		transform.basis = rot * transform.basis;
		transform.orthonormalize();
	}

	Vector3 total_linear_velocity = linear_velocity + biased_linear_velocity;
	/*for(int i=0;i<3;i++) {
		if (axis_lock&(1<<i)) {
			transform.origin[i]=0.0;
		}
	}*/

	transform.origin += total_linear_velocity * p_step;

	_set_transform(transform);
	_set_inv_transform(get_transform().inverse());

	_update_transform_dependant();

	/*
	if (fi_callback) {
		get_space()->body_add_to_state_query_list(&direct_state_query_list);
	*/
}

   void Init(CompositeNode* root, MeshFileLoader* MeshLoader )
   {
      // glDisable(GL_CULL_FACE);
      // glEnable(GL_CULL_FACE);
      // glCullFace(GL_BACK);
      // m = mfl->Load("Objects/cube.obj");
      // g = new Geometry(m, "Geometry");
      // g->SetMesh( m );
      
      Mesh* Magnolia = MeshLoader->Load("Objects/magnolia.obj");
      Magnolia->Scale(0.03f);
      Mesh* Rose = MeshLoader->Load("Objects/rose+vase.obj");
      Rose->Scale(0.03f);
      //Mesh* Dolphins = MeshLoader->Load("Objects/dolphins.obj");
	//Dolphins->Scale(0.01f);
      Mesh* Skyscraper = MeshLoader->Load("Objects/skyscraper.obj");
      Skyscraper->Scale(0.04f);
      
        


        M3DVector3f pos;
        m3dLoadVector3( pos, 0.0f, 0.0f, 50.0f );
        if (_camera) DLOG(INFO) << "_camera address: " << _camera << endl;
       
        _camera->SetPosition( pos );
         Vector4 color( 1.0f, 1.0f, 1.0f, 1.0f );
      	 _light->SetDiffuse( color );
         _light->SetAmbient( color );

	// M3DVector3f pos;
	// m3dLoadVector3( pos, 0.0f, 0.0f, 10.0f );

	// Init Camera
    //     Camera* c = reinterpret_cast<Camera*>(l->GetByName("GlobalCamera"));
    // c->SetPosition( pos );
    // c->SetPerspective(45.0f,(GLfloat)800/(GLfloat)600,0.1f,100.0f);

	// Build Up Scenegraph
	 _camera->AddChild(
	 	(new Transform(string("MagnoliaTransform")))->AddChild(
	 		(new Geometry(Magnolia, string("Magnolia")))
	 ));
         _camera->AddChild(
	 	(new Transform(string("RoseTransform")))->AddChild(
	 		(new Geometry(Rose, "Rose")))
	 );
         _camera->AddChild(
	 	(new Transform(string("SkyscraperTransform")))->AddChild(
	 		(new Geometry(Skyscraper, "Skyscraper")))
	 );

	 Transform* t = dynamic_cast<Transform*>(root->GetByName("MagnoliaTransform"));
	 t->Rotate(20.0f, 1.0f, 1.0f, 1.0f);
	 t->Translate(-3.0f, 0.0f, 0.0f);

	 t = dynamic_cast<Transform*>(root->GetByName("RoseTransform"));
	 t->Translate(2.0f, 0.0f, 0.0f);
	 t->Rotate(20.0f, 1.0f, 1.0f, 1.0f);
	 t->Scale(1.6f, 1.2f, 1.2f);

	 t = dynamic_cast<Transform*>(root->GetByName("SkyscraperTransform"));
	 t->Rotate(50.0f, 1.0f, 1.0f, 1.0f);

         update = t;

         UpdateVisitorFactory fact;
        
        visitor = fact.CreateTransformationVisitor( ROTATE, 1.0f, 0.0f, 0.0f, 0.01f );
        visitor2 = fact.CreateTransformationVisitor( SCALE, 1.002f, 1.0f, 1.0f );
   };

bool Tween::_calc_delta_val(const Variant& p_initial_val, const Variant& p_final_val, Variant& p_delta_val) {

	const Variant& initial_val = p_initial_val;
	const Variant& final_val = p_final_val;
	Variant& delta_val = p_delta_val;

	switch(initial_val.get_type()) {

		case Variant::BOOL:
			//delta_val = p_final_val;
			delta_val = (int) p_final_val - (int) p_initial_val;
			break;

		case Variant::INT:
			delta_val = (int) final_val - (int) initial_val;
			break;

		case Variant::REAL:
			delta_val = (real_t) final_val - (real_t) initial_val;
			break;

		case Variant::VECTOR2:
			delta_val = final_val.operator Vector2() - initial_val.operator Vector2();
			break;

		case Variant::VECTOR3:
			delta_val = final_val.operator Vector3() - initial_val.operator Vector3();
			break;

		case Variant::MATRIX3:
			{
				Matrix3 i = initial_val;
				Matrix3 f = final_val;
				delta_val = Matrix3(f.elements[0][0] - i.elements[0][0],
					f.elements[0][1] - i.elements[0][1],
					f.elements[0][2] - i.elements[0][2],
					f.elements[1][0] - i.elements[1][0],
					f.elements[1][1] - i.elements[1][1],
					f.elements[1][2] - i.elements[1][2],
					f.elements[2][0] - i.elements[2][0],
					f.elements[2][1] - i.elements[2][1],
					f.elements[2][2] - i.elements[2][2]
				);
			}
			break;

		case Variant::MATRIX32:
			{
				Matrix32 i = initial_val;
				Matrix32 f = final_val;
				Matrix32 d = Matrix32();
				d[0][0] = f.elements[0][0] - i.elements[0][0];
				d[0][1] = f.elements[0][1] - i.elements[0][1];
				d[1][0] = f.elements[1][0] - i.elements[1][0];
				d[1][1] = f.elements[1][1] - i.elements[1][1];
				d[2][0] = f.elements[2][0] - i.elements[2][0];
				d[2][1] = f.elements[2][1] - i.elements[2][1];
				delta_val = d;
			}
			break;
		case Variant::QUAT:
			delta_val = final_val.operator Quat() - initial_val.operator Quat();
			break;
		case Variant::_AABB:
			{
				AABB i = initial_val;
				AABB f = final_val;
				delta_val = AABB(f.pos - i.pos, f.size - i.size);
			}
			break;
		case Variant::TRANSFORM:
			{
				Transform i = initial_val;
				Transform f = final_val;
				Transform d;
				d.set(f.basis.elements[0][0] - i.basis.elements[0][0],
					f.basis.elements[0][1] - i.basis.elements[0][1],
					f.basis.elements[0][2] - i.basis.elements[0][2],
					f.basis.elements[1][0] - i.basis.elements[1][0],
					f.basis.elements[1][1] - i.basis.elements[1][1],
					f.basis.elements[1][2] - i.basis.elements[1][2],
					f.basis.elements[2][0] - i.basis.elements[2][0],
					f.basis.elements[2][1] - i.basis.elements[2][1],
					f.basis.elements[2][2] - i.basis.elements[2][2],
					f.origin.x - i.origin.x,
					f.origin.y - i.origin.y,
					f.origin.z - i.origin.z
				);

				delta_val = d;
			}
			break;
		case Variant::COLOR:
			{
				Color i = initial_val;
				Color f = final_val;
				delta_val = Color(f.r - i.r, f.g - i.g, f.b - i.b, f.a - i.a);
			}
			break;

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

void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant &p_variant) {

	switch (p_state) {
		case PhysicsServer::BODY_STATE_TRANSFORM: {

			if (mode == PhysicsServer::BODY_MODE_KINEMATIC) {
				new_transform = p_variant;
				//wakeup_neighbours();
				set_active(true);
				if (first_time_kinematic) {
					_set_transform(p_variant);
					_set_inv_transform(get_transform().affine_inverse());
					first_time_kinematic = false;
				}

			} else if (mode == PhysicsServer::BODY_MODE_STATIC) {
				_set_transform(p_variant);
				_set_inv_transform(get_transform().affine_inverse());
				wakeup_neighbours();
			} else {
				Transform t = p_variant;
				t.orthonormalize();
				new_transform = get_transform(); //used as old to compute motion
				if (new_transform == t)
					break;
				_set_transform(t);
				_set_inv_transform(get_transform().inverse());
			}
			wakeup();

		} break;
		case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {

			/*
			if (mode==PhysicsServer::BODY_MODE_STATIC)
				break;
			*/
			linear_velocity = p_variant;
			wakeup();
		} break;
		case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
			/*
			if (mode!=PhysicsServer::BODY_MODE_RIGID)
				break;
			*/
			angular_velocity = p_variant;
			wakeup();

		} break;
		case PhysicsServer::BODY_STATE_SLEEPING: {
			//?
			if (mode == PhysicsServer::BODY_MODE_STATIC || mode == PhysicsServer::BODY_MODE_KINEMATIC)
				break;
			bool do_sleep = p_variant;
			if (do_sleep) {
				linear_velocity = Vector3();
				//biased_linear_velocity=Vector3();
				angular_velocity = Vector3();
				//biased_angular_velocity=Vector3();
				set_active(false);
			} else {
				if (mode != PhysicsServer::BODY_MODE_STATIC)
					set_active(true);
			}
		} break;
		case PhysicsServer::BODY_STATE_CAN_SLEEP: {
			can_sleep = p_variant;
			if (mode == PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep)
				set_active(true);

		} break;
	}
}

void ConvexPolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {


	int vertex_count=mesh.vertices.size();
	if (vertex_count==0)
		return;

	const Vector3 *vrts=&mesh.vertices[0];
    #ifndef NEON
	for (int i=0;i<vertex_count;i++) {

		float d=p_normal.dot( p_transform.xform( vrts[i] ) );

		if (i==0 || d > r_max)
			r_max=d;
		if (i==0 || d < r_min)
			r_min=d;
	}
    #else
    int i;
    Matrix3 m = p_transform.get_basis();
    Vector3 o = p_transform.get_origin();
    float32x4_t vo[4] = {{o[0],o[0],o[0],o[0]} , {o[1],o[1],o[1],o[1]} , {o[2],o[2],o[2],o[2]} , {o[3],o[3],o[3],o[3]}};
    for (i=0;i<vertex_count-4;i+=4) { // as long as 4 calculations at a time are possible
            /*_FORCE_INLINE_ Vector3 Transform::xform(const Vector3& p_vector) const {

            return Vector3(
                basis[0].dot(p_vector)+origin.x,
                basis[1].dot(p_vector)+origin.y,
                basis[2].dot(p_vector)+origin.z
            );
            
            }*/
            //print_line("yay");
            //float d1, d2, d3, d4;
            
            
            
            //float f1_1, f1_2, f1_3, f1_4, f2_1, f2_2, f2_3, f2_4, f3_1, f3_2, f3_3, f3_4;
            float32x4_t f1, f2, f3;
            float32x4_t d;
            float32x4_t vrts_x = {vrts[i].x, vrts[i+1].x, vrts[i+2].x, vrts[i+3].x};
            float32x4_t vrts_y = {vrts[i].y, vrts[i+1].y, vrts[i+2].y, vrts[i+3].y};
            float32x4_t vrts_z = {vrts[i].z, vrts[i+1].z, vrts[i+2].z, vrts[i+3].z};
            
            /*f1_1 = m[0][0]*vrts[i][0];
            f1_2 = m[0][0]*vrts[i+1][0];
            f1_3 = m[0][0]*vrts[i+2][0];
            f1_4 = m[0][0]*vrts[i+3][0];*/
            
            //f1 = vrts_x * m[0][0];
            f1 = vmulq_n_f32(vrts_x, m[0][0]);
            
            /*f2_1 = m[1][0]*vrts[i][0];
            f2_2 = m[1][0]*vrts[i+1][0];
            f2_3 = m[1][0]*vrts[i+2][0];
            f2_4 = m[1][0]*vrts[i+3][0];*/
            
            //f2 = m[1][0] * vrts_x;
            f2 = vmulq_n_f32(vrts_x, m[1][0]);
            
            /*f3_1 = m[2][0]*vrts[i][0];
            f3_2 = m[2][0]*vrts[i+1][0];
            f3_3 = m[2][0]*vrts[i+2][0];
            f3_4 = m[2][0]*vrts[i+3][0];*/
            
            //f3 = m[2][0] * vrts_x;
            f3 = vmulq_n_f32(vrts_x, m[2][0]);
            
            /*f1_1 += m[0][1]*vrts[i][1];
            f1_2 += m[0][1]*vrts[i+1][1];
            f1_3 += m[0][1]*vrts[i+2][1];
            f1_4 += m[0][1]*vrts[i+3][1];*/
            
            //f1 += m[0][1] * vrts_y;
            f1 += vmulq_n_f32(vrts_y, m[0][1]);
            
            /*f2_1 += m[1][1]*vrts[i][1];
            f2_2 += m[1][1]*vrts[i+1][1];
            f2_3 += m[1][1]*vrts[i+2][1];
            f2_4 += m[1][1]*vrts[i+3][1];*/
            
            //f2 += m[1][1] * vrts_y;
            f2 += vmulq_n_f32(vrts_y, m[1][1]);
            
            /*f3_1 += m[2][1]*vrts[i][1];
            f3_2 += m[2][1]*vrts[i+1][1];
            f3_3 += m[2][1]*vrts[i+2][1];
            f3_4 += m[2][1]*vrts[i+3][1];*/
            
            //f3 += m[2][1] * vrts_y;
            f3 += vmulq_n_f32(vrts_y, m[2][1]);
            
            
            /*f1_1 += m[0][2]*vrts[i][2];
            f1_2 += m[0][2]*vrts[i+1][2];
            f1_3 += m[0][2]*vrts[i+2][2];
            f1_4 += m[0][2]*vrts[i+3][2];*/
            
            //f1 += m[0][2] * vrts_z;
//.........这里部分代码省略.........

void HeightMapShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {

	//not very useful, but not very used either
	p_transform.xform(get_aabb()).project_range_in_plane( Plane(p_normal,0),r_min,r_max );

}

void ParticleSystemDrawInfoSW::prepare(const ParticleSystemSW *p_system,const ParticleSystemProcessSW *p_process,const Transform& p_system_transform,const Transform& p_camera_transform) {

	ERR_FAIL_COND(p_process->particle_data.size() != p_system->amount);
	ERR_FAIL_COND(p_system->amount<=0 || p_system->amount>=ParticleSystemSW::MAX_PARTICLES);

	const ParticleSystemProcessSW::ParticleData *pdata=&p_process->particle_data[0];
	float time_pos=p_process->particle_system_time/p_system->particle_vars[VS::PARTICLE_LIFETIME];

	ParticleSystemSW::ColorPhase cphase[VS::MAX_PARTICLE_COLOR_PHASES];

	float last=-1;
	int col_count=0;

	for(int i=0;i<p_system->color_phase_count;i++) {

		if (p_system->color_phases[i].pos<=last)
			break;
		cphase[i]=p_system->color_phases[i];
		col_count++;
	}





	Vector3 camera_z_axis = p_camera_transform.basis.get_axis(2);

	for(int i=0;i<p_system->amount;i++) {

		ParticleDrawInfo &pdi=draw_info[i];
		pdi.data=&pdata[i];
		pdi.transform.origin=pdi.data->pos;
		if (p_system->local_coordinates)
			pdi.transform.origin=p_system_transform.xform(pdi.transform.origin);

		pdi.d=-camera_z_axis.dot(pdi.transform.origin);

		// adjust particle size, color and rotation

		float time = ((float)i / p_system->amount);
		if (time<time_pos)
			time=time_pos-time;
		else
			time=(1.0-time)+time_pos;

		Vector3 up=p_camera_transform.basis.get_axis(1); // up determines the rotation
		float up_scale=1.0;

		if (p_system->height_from_velocity) {

			Vector3 veld = pdi.data->vel;
			Vector3 cam_z = camera_z_axis.normalized();
			float vc = Math::abs(veld.normalized().dot(cam_z));

			if (vc<(1.0-CMP_EPSILON)) {
				up = Plane(cam_z,0).project(veld).normalized();
				float h = p_system->particle_vars[VS::PARTICLE_HEIGHT]+p_system->particle_randomness[VS::PARTICLE_HEIGHT]*pdi.data->random[7];
				float velh = veld.length();
				h+=velh*(p_system->particle_vars[VS::PARTICLE_HEIGHT_SPEED_SCALE]+p_system->particle_randomness[VS::PARTICLE_HEIGHT_SPEED_SCALE]*pdi.data->random[7]);


				up_scale=Math::lerp(1.0,h,(1.0-vc));
			}

		} else if (pdi.data->rot) {

			up.rotate(camera_z_axis,pdi.data->rot);
		}

		{
			// matrix
			Vector3 v_z = (p_camera_transform.origin-pdi.transform.origin).normalized();
//			Vector3 v_z = (p_camera_transform.origin-pdi.data->pos).normalized();
			Vector3 v_y = up;
			Vector3 v_x = v_y.cross(v_z);
			v_y = v_z.cross(v_x);
			v_x.normalize();
			v_y.normalize();


			float initial_scale, final_scale;
			initial_scale = p_system->particle_vars[VS::PARTICLE_INITIAL_SIZE]+p_system->particle_randomness[VS::PARTICLE_INITIAL_SIZE]*pdi.data->random[5];
			final_scale = p_system->particle_vars[VS::PARTICLE_FINAL_SIZE]+p_system->particle_randomness[VS::PARTICLE_FINAL_SIZE]*pdi.data->random[6];
			float scale = initial_scale + time * (final_scale - initial_scale);

			pdi.transform.basis.set_axis(0,v_x * scale);
			pdi.transform.basis.set_axis(1,v_y * scale * up_scale);
			pdi.transform.basis.set_axis(2,v_z * scale);
		}



		int cpos=0;

		while(cpos<col_count) {

			if (cphase[cpos].pos > time)
				break;
			cpos++;
		}
//.........这里部分代码省略.........

// Given a point Q on an ellipsoid, with outward unit normal nn at Q: find the 
// principal curvatures at the point and their directions. The result is a 
// coordinate frame with origin Q, z axis the ellipsoid normal nn at Q, x axis 
// is the direction dmax of maximum curvature kmax, y axis the direction dmin 
// of minimum curvature kmin, such that [dmax dmin n] forms a right-handed set.
// This is equivalent to fitting an elliptic paraboloid 
// z = -kmax/2 x^2 -kmin/2 y^2 to the ellipsoid at point Q. Note that for
// an ellipsoid we have kmax>=kmin>0.
//
// We'll find the ellipse on the central plane perpendicular to the normal by 
// intersecting the plane equation with the ellipsoid equation but working in 
// the plane frame P=[u v n], where u and v are arbitrary axes in the plane.
// Our goal is to obtain an equation for the ellipse in P and then rotate the 
// P frame about its normal until we get the ellipse in standard form 
// Ru^2+Sv^2=1 in which case d/R and d/S are the ellipsoid curvatures (d is the
// distance from the point on the ellipsoid to the plane).
// ref: McArthur, Neil. "Principal radii of curvature at a point on an 
// ellipsoid", Mathematical Notes 24 pp. xvi-xvii, 1929.
//
// In its own frame E=[x y z] the ellipsoid surface is the set of points such 
// that
//    ~e * diag(A,B,C) * e = 1
// where e is a vector expressed in E. The plane is the set of points 
// satisfying ~e * n = 0. We can write rotation matrix R_EP=[u v n] where 
// u,v,n are expressed in E. Now we can put the ellipsoid in P:
//   ~(R_EP*p) * diag(A,B,C) * (R_EP*p) = 1
// We can intersect that with the plane just by dropping the n coordinate of 
// p so p=[u v 0] (u,v scalars here), and the intersection equation is
//    A(u*ux + v*vx)^2 + B(u*uy+v*vy)^2 + C(u*uz + v*vz)^2 = 1
// which is
//    R u^2 + S v^2 + T u*v = 1
// with
//    R =   A ux^2  + B uy^2  + C uz^2
//    S =   A vx^2  + B vy^2  + C vz^2
//    T = 2(A ux*vx + B uy*vy + C uz*vz)
//
// We want to find a rotation about n that eliminates the cross term Tuv, 
// leaving us with
//    R' u'^2 + S' v'^2 = 1
// for new constants R' and S' and new basis u' and v'.
//
// Method
// ------
// We'll calculate an angle theta where theta=0 would be along u and 
// theta=pi/2 would be along v. Then theta+pi/2 is a perpendicular direction 
// that has the other curvature extreme. Per "Dr Rob" at Mathforum.org 2000:
//   t2t = tan(2*theta) = T/(R-S)
//   theta = atan(t2t)/2, c = cos(theta), s = sin(theta)
//   R' = Rc^2 + Tsc + Ss^2   (theta direction)
//   S' = Rs^2 - Tsc + Sc^2   (theta+pi/2 direction)
// Directions are u' = c*u + s*v, v' = c*v - s*u; these are automatically unit
// vectors.
//
// Optimization
// ------------
// The above requires an atan() to get 2*theta then sin & cos(theta) at
// a cost of about 120 flops. We can use half angle formulas to work
// exclusively with 2*theta, but then we'll have to normalize u' and v' 
// at the end:
//   t2t = tan(2*theta) = T/(R-S)
//   c2t = cos(2*theta) = 1/sqrt(1 + t2t^2)
//   s2t = sin(2*theta) = t2t*cos2t;
//   2*R' = R+S + Rc2t - Sc2t + Ts2t
//   2*S' = R+S - Rc2t + Sc2t - Ts2t
// By multiplying the u',v' formulas above by 2*c we change the lengths
// but get expressions that are easily converted to double angles:
//   u' = normalize((1+c2t)*u + s2t*v)
//   v' = normalize((1+c2t)*v - s2t*u)
// (but actually v' is n X u' which is cheap). This saves about 30 
// flops over the straightforward method above.
//
// Cost: given a point and normalized normal
//    curvatures ~160 flops
//    directions ~ 60 flops more
//               ----
//               ~220 flops
//
// So: Given an ellipsoid in its own frame E, with equation Ax^2+By^2+Cz^2=1, a 
// point Q=(x,y,z) on its surface, and the unit outward normal vector nn at Q,
// return (kmax,kmin) the principal curvatures at Q, and a Transform with 
// x=dmax, y=dmin, z=nn, O=Q that gives the principal curvature directions. 
// (Note: A=1/a^2, B=1/b^2, C=1/c^2 where a,b,c are the ellipsoid radii.)
void ContactGeometry::Ellipsoid::Impl::
findParaboloidAtPointWithNormal(const Vec3& Q, const UnitVec3& nn,
                                Transform& X_EP, Vec2& k) const
{
    const Real A = square(curvatures[0]), B = square(curvatures[1]), 
               C = square(curvatures[2]);

    // Sanity checks in debug.
    SimTK_ERRCHK(std::abs(A*Q[0]*Q[0]+B*Q[1]*Q[1]+C*Q[2]*Q[2]-1) < SqrtEps,
        "ContactGeometry::Ellipsoid::findParaboloidAtPointWithNormal()",
        "The given point was not on the surface of the ellipsoid.");
    SimTK_ERRCHK((nn-findUnitNormalAtPoint(Q)).normSqr() < SqrtEps,
        "ContactGeometry::Ellipsoid::findParaboloidAtPointWithNormal()",
        "The given normal was not consistent with the given point.");

    UnitVec3 tu = nn.perp();    // ~40 flops
    UnitVec3 tv(nn % tu, true); // y = z X x for plane, already normalized (9 flops)
    
//.........这里部分代码省略.........

 /*
  * Shortcut to transform [vector]  by [matrix]. This transformation occurs in world space.
  */
 void SceneObjectTransform::TransformVector(Vector3& vector) const {
     Transform full;
     GetInheritedTransform(full, false);
     full.TransformBy(this);
     full.TransformVector(vector);
 }