Ejemplo n.º 1
0
        public bool intersect(Vector3 source, Vector3 ray,
            out float distance, out System.Drawing.Color color)
        {
            Vector3 s = m_center - source;
            float rayDotS = Vector3.Dot(ray, s);

            float discr = 4.0f * (float)(Math.Pow(rayDotS, 2) - Math.Pow(ray.Length(), 2)
                * (Math.Pow(s.Length(), 2) - Math.Pow(m_radius, 2)));

            if (discr >= 0)
            {
                float t1 = (float)(rayDotS + Math.Sqrt(discr)
                    / (2.0f * Math.Pow(ray.Length(), 2)));
                float t2 = (float)(rayDotS - Math.Sqrt(discr)
                    / (2.0f * Math.Pow(ray.Length(), 2)));

                if (t2 > 0)
                {
                    distance = t2;
                }
                else
                {
                    distance = t1;
                }
            }
            else
            {
                distance = 0;
            }

            color = m_color;

            return (discr >= 0);
        }
Ejemplo n.º 2
0
        protected override void OnMouseDown(MouseEventArgs e)
        {
            _time = DateTime.Now;

            if (e.Button == MouseButtons.Left)
                _pts[0] = e.Location;
            if (e.Button == MouseButtons.Middle)
                _pts[1] = e.Location;
            if (e.Button == MouseButtons.Right)
                _pts[2] = e.Location;

            var v0 = new Vector3(_pts[1].X - _pts[0].X, 0, _pts[1].Y - _pts[0].Y);
            var v1 = new Vector3(_pts[1].X - _pts[2].X, 0, _pts[1].Y - _pts[2].Y);
            _out = Vector3.Cross(v0, v1);
            var sinAngle = _out.Length()/(v0.Length()*v1.Length());
            _angle = (float)Math.Asin(sinAngle);
            System.Diagnostics.Debug.Print("{0:0.0}", MathUtil.RadiansToDegrees(_angle));

            var v2 = Vector3.Cross(v1, _out);
            v2.Normalize();
            System.Diagnostics.Debug.Print("v2:{0}  d:{1}", v2, v1.Length());
            v2 *= v1.Length() / (float)Math.Tan(Math.PI - _angle) / 2;

            _pts[3] = new PointF(v2.X + (_pts[2].X + _pts[1].X)/2, v2.Z + (_pts[2].Y + _pts[1].Y)/2);

            copyToCamera();
            System.Diagnostics.Debug.Print("MC {0:0.0}", MathUtil.RadiansToDegrees(_moveCameraArc._angle));

            Invalidate();
        }
Ejemplo n.º 3
0
        public void Vector3_CalculatesLengthCorrectly()
        {
            var vector = new Vector3(123.4f, 567.8f, 901.2f);

            TheResultingValue(vector.Length()).WithinDelta(0.1f)
                .ShouldBe((float)Math.Sqrt((123.4f * 123.4f) + (567.8f * 567.8f) + (901.2f * 901.2f)));
        }
Ejemplo n.º 4
0
        protected override Vector3 ArrangeOverride(Vector3 finalSizeWithoutMargins)
        {
            var maxLength = Math.Max(finalSizeWithoutMargins.Length(), ExpectedArrangeValue.Length());
            Assert.IsTrue((finalSizeWithoutMargins - ExpectedArrangeValue).Length() <= maxLength * 0.001f);

            return base.ArrangeOverride(finalSizeWithoutMargins);
        }
Ejemplo n.º 5
0
        public override float GetDistanceToCenter(
            Vector3 particlePosition, Vector3 particleVelocity,
            out Vector3 alongAxis, out Vector3 aroundAxis, out Vector3 awayAxis)
        {
            alongAxis = new Vector3(0, 1, 0);

            particlePosition -= fieldPosition;            
            inverseRotation.Rotate(ref particlePosition);
            particlePosition /= fieldSize;

            // Start by positioning hte particle on the torus' plane
            var projectedPosition = new Vector3(particlePosition.X, 0, particlePosition.Z);
            var distanceFromOrigin = projectedPosition.Length();
            var distSquared = 1 + distanceFromOrigin * distanceFromOrigin - 2 * distanceFromOrigin + particlePosition.Y * particlePosition.Y;

            var totalStrength = (distSquared >= smallRadiusSquared) ? 1 : ((float) Math.Sqrt(distSquared) / smallRadius);

            // Fix the field's axis back to world space
            var forceAxis = Vector3.Cross(alongAxis, projectedPosition);
            fieldRotation.Rotate(ref forceAxis);
            forceAxis.Normalize();
            alongAxis = forceAxis;

            projectedPosition = (distanceFromOrigin > 0) ? (projectedPosition/(float)distanceFromOrigin) : projectedPosition;
            projectedPosition -= particlePosition;
            projectedPosition *= fieldSize;
            fieldRotation.Rotate(ref projectedPosition);
            awayAxis = -projectedPosition;
            awayAxis.Normalize();

            aroundAxis = Vector3.Cross(alongAxis, awayAxis);

            return totalStrength;
        }
 public void SetRelativeCameraPos(Vector3 cameraPos)
 {
     m_D3DEffect.SetValue(m_cameraPos, cameraPos);
     float height = cameraPos.Length();
     m_D3DEffect.SetValue(m_cameraHeight2, height * height);
     m_D3DEffect.SetValue(m_cameraHeight, height);
 }
Ejemplo n.º 7
0
 /// <summary>
 /// 拡大縮小ベクトルと回転行列と位置ベクトルに分割します。
 /// </summary>
 /// <param name="m">元の行列(戻り値は回転行列)</param>
 /// <param name="scaling">拡大縮小ベクトル</param>
 /// <returns>位置ベクトル</returns>
 public static Vector3 DecomposeMatrix(ref Matrix m, out Vector3 scaling)
 {
     Vector3 vx = new Vector3(m.M11, m.M12, m.M13);
     Vector3 vy = new Vector3(m.M21, m.M22, m.M23);
     Vector3 vz = new Vector3(m.M31, m.M32, m.M33);
     Vector3 vt = new Vector3(m.M41, m.M42, m.M43);
     float scax = vx.Length();
     float scay = vy.Length();
     float scaz = vz.Length();
     scaling = new Vector3(scax, scay, scaz);
     vx.Normalize();
     vy.Normalize();
     vz.Normalize();
     m.M11 = vx.X;
     m.M12 = vx.Y;
     m.M13 = vx.Z;
     m.M21 = vy.X;
     m.M22 = vy.Y;
     m.M23 = vy.Z;
     m.M31 = vz.X;
     m.M32 = vz.Y;
     m.M33 = vz.Z;
     m.M41 = 0;
     m.M42 = 0;
     m.M43 = 0;
     return vt;
 }
Ejemplo n.º 8
0
		public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform)
		{
			predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep;
			//exponential map
			Vector3 axis;
			float angle = angularVelocity.Length();
			//limit the angular motion
			if (angle * timeStep > AngularMotionTreshold)
			{
				angle = AngularMotionTreshold / timeStep;
			}

			if (angle < 0.001f)
			{
				// use Taylor's expansions of sync function
				axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle);
			}
			else
			{
				// sync(fAngle) = sin(c*fAngle)/t
				axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle);
			}
			Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f));
			Quaternion ornA = MatrixOperations.GetRotation(currentTransform);

			Quaternion predictedOrn = dorn * ornA;
			predictedOrn.Normalize();

			MatrixOperations.SetRotation(ref predictedTransform, predictedOrn);

			Matrix test = Matrix.CreateFromQuaternion(predictedOrn);
		}
Ejemplo n.º 9
0
 public MyVoxelMapImpostor(Vector3 position, float radius, float angle)
 {
     Position = position;
     Radius = radius;
     Angle = angle;
     m_distance = position.Length();
 }
Ejemplo n.º 10
0
        public Cylinder(float[] Begin, float[] End, float radius1, float radius2, int slices, int stacks, Device dev)
        {
            float_coordinates = new float[2][];
            float_coordinates[0] = Begin;
            float_coordinates[1] = End;
            device = dev;
            if ((Begin.Length != Basis.Length) || (End.Length != Basis.Length))
                throw new Exception("Размерность базиса не совпадает с размерностью вектора позиции.");
            Vector3 begin = new Vector3(0.0f, 0.0f, 0.0f);
            Vector3 end = new Vector3(0.0f, 0.0f, 0.0f);
            for (int i = 0; i < Basis.Length; i++)
            {
                begin += Begin[i] * Basis[i];
                end += End[i] * Basis[i];
            }

            MovingMatrix = Matrix.Translation(new Vector3(0, 0, (begin - end).Length() / 2));
            Vector3 be = end - begin;
            Vector3 curpos = new Vector3(0, 0, (begin - end).Length());
            float angle = (float)Math.Acos(VectorActions.scalmul(be, curpos) / (be.Length() * curpos.Length()));
            Vector3 axis = VectorActions.vectmul(be, curpos);

            MovingMatrix *= Matrix.RotationAxis(axis, -angle);
            MovingMatrix *= Matrix.Translation(begin);

            meshes = new Mesh[1];
            meshes[0] = Mesh.Cylinder(device, radius1, radius2, (begin - end).Length(), slices, stacks);
            meshes[0].ComputeNormals();
            material = new Material();
        }
Ejemplo n.º 11
0
 /// <summary>
 /// Launch a particle
 /// </summary>
 /// <param name="position">Position the particle</param>
 /// <param name="velocity">The velocity vector</param>
 /// <param name="timeLife">The expiration time of the particle</param>
 public void LaunchParticle(Vector3 position, Vector3 velocity, float timeLife)
 {
     this.Position = position;
     this.Velocity = velocity;
     this.VelocityLength = velocity.Length();
     this.LifeTime = timeLife;
     this.IsAlive = true;
 }
Ejemplo n.º 12
0
        protected override Vector3 ArrangeOverride(Vector3 finalSizeWithoutMargins)
        {
            var maxLength = Math.Max(finalSizeWithoutMargins.Length(), ExpectedArrangeValue.Length());
            Assert.IsTrue((finalSizeWithoutMargins - ExpectedArrangeValue).Length() <= maxLength * 0.001f, 
                "Arrange validator test failed: expected value=" + ExpectedArrangeValue + ", Received value=" + finalSizeWithoutMargins + " (Validator='" + Name + "'");

            return base.ArrangeOverride(finalSizeWithoutMargins);
        }
Ejemplo n.º 13
0
		public static double GetRotationAngle(
			Vector3 rotationStatus,
			double rotationValue,
			Vector3 rotationAxis)
		{
			double angle = 0.0;

			if (rotationStatus.Dot (rotationAxis) >= 0.0) 
			{
				angle = 2.0 * Math.Atan2 (rotationStatus.Length (), rotationValue);
			} 
			else 
			{
				angle = 2.0 * Math.Atan2 (rotationStatus.Length (), -rotationValue);
			}

			return (angle > Math.PI) ? angle - 2.0 * Math.PI : angle;
		}
Ejemplo n.º 14
0
        /// <summary>
        /// Gets the shortest distance between the given point and the given line.
        /// </summary>
        /// <param name="ray">Ray to build the line from.</param>
        /// <param name="origin">Origin point to build the line from.</param>
        /// <param name="point">Point to compare the distance from.</param>
        /// <returns>The shortest distance from the point.</returns>
        public static float Distance_PointToLine( Vector3 ray, Vector3 origin, Vector3 point )
        {
            if ( ray.Length() < 0.0f )
                return -1.0f;

            Vector3 pointVector = point - origin;
            double theta;
            float length = pointVector.Length();

            if ( length < 0.0f )
                return -1.0f;

            if ( double.IsNaN( Math.Acos( Vector3.Dot( ray, pointVector ) / ( ray.Length() * length ) ) ) )
                theta = 0;
            else
                theta = Math.Acos( Vector3.Dot( ray, pointVector ) / ( ray.Length() * length ) );

            return length * ( float ) Math.Sin( theta );
        }
Ejemplo n.º 15
0
        public static Vector3 Limit(Vector3 initial, float maxLen)
        {
            float currLen = initial.Length ();
            float ratio = 1.0f;

            if (currLen > maxLen) {
                ratio = currLen / maxLen;
            }

            return initial /= ratio;
        }
Ejemplo n.º 16
0
		/// <summary>
		/// Создает камеру, которую можно крутить движением мышки
		/// </summary>
		/// <param name="eventSource">Control,с которого обрабатываются события </param>
		/// <param name="cameraLocation">Исходное положение камеры</param>
		public TrackballCamera(Control eventSource,Frame3D cameraLocation)
			: base(eventSource.ClientSize.Width/(double) eventSource.ClientSize.Height)
		{
			EventSource = eventSource;
			Vector3 tempLoc = cameraLocation.ToDirectXVector();
			_cameraLocation = new Vector3(Math.Abs(tempLoc.X), 0, tempLoc.Z);
            Angle ang = Geometry.Atan2(tempLoc.Y, tempLoc.X);
			_rotationMatrix = Matrix.RotationZ(-(float) ang.Radian);
			_radius = _cameraLocation.Length();
			Scale = 1;
		}
Ejemplo n.º 17
0
		// solve unilateral constraint (equality, direct method)
		public void ResolveUnilateralPairConstraint(
							RigidBody body1, RigidBody body2,
							Matrix world2A,
							Matrix world2B,
							Vector3 invInertiaADiag,
							float invMassA,
							Vector3 linvelA, Vector3 angvelA,
							Vector3 rel_posA1,
							Vector3 invInertiaBDiag,
							float invMassB,
							Vector3 linvelB, Vector3 angvelB,
							Vector3 rel_posA2,
							float depthA, Vector3 normalA,
							Vector3 rel_posB1, Vector3 rel_posB2,
							float depthB, Vector3 normalB,
							out float imp0, out float imp1)
		{
			imp0 = 0;
			imp1 = 0;

			float len = Math.Abs(normalA.Length()) - 1f;
			if (Math.Abs(len) >= float.Epsilon)
				return;

			BulletDebug.Assert(len < float.Epsilon);

			//this jacobian entry could be re-used for all iterations
			JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
				invInertiaBDiag, invMassB);
			JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
				invInertiaBDiag, invMassB);

			float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
			float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));

			//	btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
			float massTerm = 1f / (invMassA + invMassB);

			// calculate rhs (or error) terms
			float dv0 = depthA * _tau * massTerm - vel0 * _damping;
			float dv1 = depthB * _tau * massTerm - vel1 * _damping;

			float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
			float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);

			imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
			imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
		}
        public Plane(Vector3 pos0,Vector3 pos1,Vector3 pos2)
            : base((int)PrimitiveType.Plane)
        {
            Vector3 dr1 = pos1 - pos0;
            Vector3 dr2 = pos2 - pos0;

            this.normal = Vector3.Cross(dr1, dr2);
            float mNLen = normal.Length();
            if (mNLen < JiggleMath.Epsilon)
            {
                normal = Vector3Helper.Up; //this.normal = Vector3.Up;
                this.d = 0.0f;
            }
            else
            {
                this.normal /= mNLen;
                this.d = -Vector3.Dot(this.normal, pos0);
            }
        }
Ejemplo n.º 19
0
        public static void InitBoxPhysics(this IMyEntity entity, MyStringHash materialType, Vector3 center, Vector3 size, float mass, float linearDamping, float angularDamping, ushort collisionLayer, RigidBodyFlag rbFlag)
        {
            System.Diagnostics.Debug.Assert(size.Length() > 0);

            mass = (rbFlag & RigidBodyFlag.RBF_STATIC) != 0 ? 0 : mass;

            var massProperties = HkInertiaTensorComputer.ComputeBoxVolumeMassProperties(size / 2, mass);

            var physics = new Sandbox.Engine.Physics.MyPhysicsBody(entity, rbFlag)
            {
                MaterialType = materialType,
                AngularDamping = angularDamping,
                LinearDamping = linearDamping
            };

            HkBoxShape shape = new HkBoxShape(size * 0.5f);
            physics.CreateFromCollisionObject((HkShape)shape, center, entity.PositionComp.WorldMatrix, massProperties);
            shape.Base.RemoveReference();

            entity.Physics = physics;
        }
Ejemplo n.º 20
0
        public EntityIntersection TestIntersectionOBB(Ray iray, Quaternion parentrot, bool frontFacesOnly, bool faceCenters)
        {
            // In this case we're using a rectangular prism, which has 6 faces and therefore 6 planes
            // This breaks down into the ray---> plane equation.
            // TODO: Change to take shape into account
            Vector3[] vertexes = new Vector3[8];

            // float[] distance = new float[6];
            Vector3[] FaceA = new Vector3[6]; // vertex A for Facei
            Vector3[] FaceB = new Vector3[6]; // vertex B for Facei
            Vector3[] FaceC = new Vector3[6]; // vertex C for Facei
            Vector3[] FaceD = new Vector3[6]; // vertex D for Facei

            Vector3[] normals = new Vector3[6]; // Normal for Facei
            Vector3[] AAfacenormals = new Vector3[6]; // Axis Aligned face normals

            AAfacenormals[0] = new Vector3(1, 0, 0);
            AAfacenormals[1] = new Vector3(0, 1, 0);
            AAfacenormals[2] = new Vector3(-1, 0, 0);
            AAfacenormals[3] = new Vector3(0, -1, 0);
            AAfacenormals[4] = new Vector3(0, 0, 1);
            AAfacenormals[5] = new Vector3(0, 0, -1);

            Vector3 AmBa = new Vector3(0, 0, 0); // Vertex A - Vertex B
            Vector3 AmBb = new Vector3(0, 0, 0); // Vertex B - Vertex C
            Vector3 cross = new Vector3();

            Vector3 pos = GetWorldPosition();
            Quaternion rot = GetWorldRotation();

            // Variables prefixed with AX are Axiom.Math copies of the LL variety.

            Quaternion AXrot = rot;
            AXrot.Normalize();

            Vector3 AXpos = pos;

            // tScale is the offset to derive the vertex based on the scale.
            // it's different for each vertex because we've got to rotate it
            // to get the world position of the vertex to produce the Oriented Bounding Box

            Vector3 tScale = Vector3.Zero;

            Vector3 AXscale = new Vector3(m_shape.Scale.X * 0.5f, m_shape.Scale.Y * 0.5f, m_shape.Scale.Z * 0.5f);

            //Vector3 pScale = (AXscale) - (AXrot.Inverse() * (AXscale));
            //Vector3 nScale = (AXscale * -1) - (AXrot.Inverse() * (AXscale * -1));

            // rScale is the rotated offset to find a vertex based on the scale and the world rotation.
            Vector3 rScale = new Vector3();

            // Get Vertexes for Faces Stick them into ABCD for each Face
            // Form: Face<vertex>[face] that corresponds to the below diagram
            #region ABCD Face Vertex Map Comment Diagram
            //                   A _________ B
            //                    |         |
            //                    |  4 top  |
            //                    |_________|
            //                   C           D

            //                   A _________ B
            //                    |  Back   |
            //                    |    3    |
            //                    |_________|
            //                   C           D

            //   A _________ B                     B _________ A
            //    |  Left   |                       |  Right  |
            //    |    0    |                       |    2    |
            //    |_________|                       |_________|
            //   C           D                     D           C

            //                   A _________ B
            //                    |  Front  |
            //                    |    1    |
            //                    |_________|
            //                   C           D

            //                   C _________ D
            //                    |         |
            //                    |  5 bot  |
            //                    |_________|
            //                   A           B
            #endregion

            #region Plane Decomposition of Oriented Bounding Box
            tScale = new Vector3(AXscale.X, -AXscale.Y, AXscale.Z);
            rScale = tScale * AXrot;
            vertexes[0] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));
               // vertexes[0].X = pos.X + vertexes[0].X;
            //vertexes[0].Y = pos.Y + vertexes[0].Y;
            //vertexes[0].Z = pos.Z + vertexes[0].Z;

            FaceA[0] = vertexes[0];
            FaceB[3] = vertexes[0];
            FaceA[4] = vertexes[0];

            tScale = AXscale;
            rScale = tScale * AXrot;
            vertexes[1] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));

               // vertexes[1].X = pos.X + vertexes[1].X;
               // vertexes[1].Y = pos.Y + vertexes[1].Y;
            //vertexes[1].Z = pos.Z + vertexes[1].Z;

            FaceB[0] = vertexes[1];
            FaceA[1] = vertexes[1];
            FaceC[4] = vertexes[1];

            tScale = new Vector3(AXscale.X, -AXscale.Y, -AXscale.Z);
            rScale = tScale * AXrot;

            vertexes[2] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));

            //vertexes[2].X = pos.X + vertexes[2].X;
            //vertexes[2].Y = pos.Y + vertexes[2].Y;
            //vertexes[2].Z = pos.Z + vertexes[2].Z;

            FaceC[0] = vertexes[2];
            FaceD[3] = vertexes[2];
            FaceC[5] = vertexes[2];

            tScale = new Vector3(AXscale.X, AXscale.Y, -AXscale.Z);
            rScale = tScale * AXrot;
            vertexes[3] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));

            //vertexes[3].X = pos.X + vertexes[3].X;
               // vertexes[3].Y = pos.Y + vertexes[3].Y;
               // vertexes[3].Z = pos.Z + vertexes[3].Z;

            FaceD[0] = vertexes[3];
            FaceC[1] = vertexes[3];
            FaceA[5] = vertexes[3];

            tScale = new Vector3(-AXscale.X, AXscale.Y, AXscale.Z);
            rScale = tScale * AXrot;
            vertexes[4] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));

               // vertexes[4].X = pos.X + vertexes[4].X;
               // vertexes[4].Y = pos.Y + vertexes[4].Y;
               // vertexes[4].Z = pos.Z + vertexes[4].Z;

            FaceB[1] = vertexes[4];
            FaceA[2] = vertexes[4];
            FaceD[4] = vertexes[4];

            tScale = new Vector3(-AXscale.X, AXscale.Y, -AXscale.Z);
            rScale = tScale * AXrot;
            vertexes[5] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));

               // vertexes[5].X = pos.X + vertexes[5].X;
               // vertexes[5].Y = pos.Y + vertexes[5].Y;
               // vertexes[5].Z = pos.Z + vertexes[5].Z;

            FaceD[1] = vertexes[5];
            FaceC[2] = vertexes[5];
            FaceB[5] = vertexes[5];

            tScale = new Vector3(-AXscale.X, -AXscale.Y, AXscale.Z);
            rScale = tScale * AXrot;
            vertexes[6] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));

               // vertexes[6].X = pos.X + vertexes[6].X;
               // vertexes[6].Y = pos.Y + vertexes[6].Y;
               // vertexes[6].Z = pos.Z + vertexes[6].Z;

            FaceB[2] = vertexes[6];
            FaceA[3] = vertexes[6];
            FaceB[4] = vertexes[6];

            tScale = new Vector3(-AXscale.X, -AXscale.Y, -AXscale.Z);
            rScale = tScale * AXrot;
            vertexes[7] = (new Vector3((pos.X + rScale.X), (pos.Y + rScale.Y), (pos.Z + rScale.Z)));

               // vertexes[7].X = pos.X + vertexes[7].X;
               // vertexes[7].Y = pos.Y + vertexes[7].Y;
               // vertexes[7].Z = pos.Z + vertexes[7].Z;

            FaceD[2] = vertexes[7];
            FaceC[3] = vertexes[7];
            FaceD[5] = vertexes[7];
            #endregion

            // Get our plane normals
            for (int i = 0; i < 6; i++)
            {
                //m_log.Info("[FACECALCULATION]: FaceA[" + i + "]=" + FaceA[i] + " FaceB[" + i + "]=" + FaceB[i] + " FaceC[" + i + "]=" + FaceC[i] + " FaceD[" + i + "]=" + FaceD[i]);

                // Our Plane direction
                AmBa = FaceA[i] - FaceB[i];
                AmBb = FaceB[i] - FaceC[i];

                cross = Vector3.Cross(AmBb, AmBa);

                // normalize the cross product to get the normal.
                normals[i] = cross / cross.Length();

                //m_log.Info("[NORMALS]: normals[ " + i + "]" + normals[i].ToString());
                //distance[i] = (normals[i].X * AmBa.X + normals[i].Y * AmBa.Y + normals[i].Z * AmBa.Z) * -1;
            }

            EntityIntersection result = new EntityIntersection();

            result.distance = 1024;
            float c = 0;
            float a = 0;
            float d = 0;
            Vector3 q = new Vector3();

            #region OBB Version 2 Experiment
            //float fmin = 999999;
            //float fmax = -999999;
            //float s = 0;

            //for (int i=0;i<6;i++)
            //{
                //s = iray.Direction.Dot(normals[i]);
                //d = normals[i].Dot(FaceB[i]);

                //if (s == 0)
                //{
                    //if (iray.Origin.Dot(normals[i]) > d)
                    //{
                        //return result;
                    //}
                   // else
                    //{
                        //continue;
                    //}
                //}
                //a = (d - iray.Origin.Dot(normals[i])) / s;
                //if (iray.Direction.Dot(normals[i]) < 0)
                //{
                    //if (a > fmax)
                    //{
                        //if (a > fmin)
                        //{
                            //return result;
                        //}
                        //fmax = a;
                    //}

                //}
                //else
                //{
                    //if (a < fmin)
                    //{
                        //if (a < 0 || a < fmax)
                        //{
                            //return result;
                        //}
                        //fmin = a;
                    //}
                //}
            //}
            //if (fmax > 0)
            //    a= fmax;
            //else
               //     a=fmin;

            //q = iray.Origin + a * iray.Direction;
            #endregion

            // Loop over faces (6 of them)
            for (int i = 0; i < 6; i++)
            {
                AmBa = FaceA[i] - FaceB[i];
                AmBb = FaceB[i] - FaceC[i];
                d = Vector3.Dot(normals[i], FaceB[i]);

                //if (faceCenters)
                //{
                //    c = normals[i].Dot(normals[i]);
                //}
                //else
                //{
                c = Vector3.Dot(iray.Direction, normals[i]);
                //}
                if (c == 0)
                    continue;

                a = (d - Vector3.Dot(iray.Origin, normals[i])) / c;

                if (a < 0)
                    continue;

                // If the normal is pointing outside the object
                if (Vector3.Dot(iray.Direction, normals[i]) < 0 || !frontFacesOnly)
                {
                    //if (faceCenters)
                    //{   //(FaceA[i] + FaceB[i] + FaceC[1] + FaceD[i]) / 4f;
                    //    q =  iray.Origin + a * normals[i];
                    //}
                    //else
                    //{
                        q = iray.Origin + iray.Direction * a;
                    //}

                    float distance2 = (float)GetDistanceTo(q, AXpos);
                    // Is this the closest hit to the object's origin?
                    //if (faceCenters)
                    //{
                    //    distance2 = (float)GetDistanceTo(q, iray.Origin);
                    //}

                    if (distance2 < result.distance)
                    {
                        result.distance = distance2;
                        result.HitTF = true;
                        result.ipoint = q;
                        //m_log.Info("[FACE]:" + i.ToString());
                        //m_log.Info("[POINT]: " + q.ToString());
                        //m_log.Info("[DIST]: " + distance2.ToString());
                        if (faceCenters)
                        {
                            result.normal = AAfacenormals[i] * AXrot;

                            Vector3 scaleComponent = AAfacenormals[i];
                            float ScaleOffset = 0.5f;
                            if (scaleComponent.X != 0) ScaleOffset = AXscale.X;
                            if (scaleComponent.Y != 0) ScaleOffset = AXscale.Y;
                            if (scaleComponent.Z != 0) ScaleOffset = AXscale.Z;
                            ScaleOffset = Math.Abs(ScaleOffset);
                            Vector3 offset = result.normal * ScaleOffset;
                            result.ipoint = AXpos + offset;

                            ///pos = (intersectionpoint + offset);
                        }
                        else
                        {
                            result.normal = normals[i];
                        }
                        result.AAfaceNormal = AAfacenormals[i];
                    }
                }
            }
            return result;
        }
Ejemplo n.º 21
0
 public Vector3 llVecNorm(IScriptInstance script, Vector3 vec)
 {
     // NOTE: Emulates behavior reported in https://jira.secondlife.com/browse/SVC-4711
     double mag = vec.Length();
     return (mag != 0.0d) ? vec / (float)mag : Vector3.Zero;
 }
Ejemplo n.º 22
0
 public double llVecMag(IScriptInstance script, Vector3 vec)
 {
     return vec.Length();
 }
Ejemplo n.º 23
0
        public void llPushObject(string target, LSL_Vector impulse, LSL_Vector ang_impulse, int local)
        {
            m_host.AddScriptLPS(1);
            bool pushrestricted = World.RegionInfo.RegionSettings.RestrictPushing;
            bool pushAllowed = false;

            bool pusheeIsAvatar = false;
            UUID targetID = UUID.Zero;

            if (!UUID.TryParse(target,out targetID))
                return;

            ScenePresence pusheeav = null;
            Vector3 PusheePos = Vector3.Zero;
            SceneObjectPart pusheeob = null;

            ScenePresence avatar = World.GetScenePresence(targetID);
            if (avatar != null)
            {
                pusheeIsAvatar = true;

                // Pushee doesn't have a physics actor
                if (avatar.PhysicsActor == null)
                    return;

                // Pushee is in GodMode this pushing object isn't owned by them
                if (avatar.GodLevel > 0 && m_host.OwnerID != targetID)
                    return;

                pusheeav = avatar;

                // Find pushee position
                // Pushee Linked?
                SceneObjectPart sitPart = pusheeav.ParentPart;
                if (sitPart != null)
                    PusheePos = sitPart.AbsolutePosition;
                else
                    PusheePos = pusheeav.AbsolutePosition;
            }

            if (!pusheeIsAvatar)
            {
                // not an avatar so push is not affected by parcel flags
                pusheeob = World.GetSceneObjectPart((UUID)target);

                // We can't find object
                if (pusheeob == null)
                    return;

                // Object not pushable.  Not an attachment and has no physics component
                if (!pusheeob.ParentGroup.IsAttachment && pusheeob.PhysActor == null)
                    return;

                PusheePos = pusheeob.AbsolutePosition;
                pushAllowed = true;
            }
            else
            {
                if (pushrestricted)
                {
                    ILandObject targetlandObj = World.LandChannel.GetLandObject(PusheePos.X, PusheePos.Y);

                    // We didn't find the parcel but region is push restricted so assume it is NOT ok
                    if (targetlandObj == null)
                        return;

                    // Need provisions for Group Owned here
                    if (m_host.OwnerID == targetlandObj.LandData.OwnerID ||
                        targetlandObj.LandData.IsGroupOwned || m_host.OwnerID == targetID)
                    {
                        pushAllowed = true;
                    }
                }
                else
                {
                    ILandObject targetlandObj = World.LandChannel.GetLandObject(PusheePos.X, PusheePos.Y);
                    if (targetlandObj == null)
                    {
                        // We didn't find the parcel but region isn't push restricted so assume it's ok
                        pushAllowed = true;
                    }
                    else
                    {
                        // Parcel push restriction
                        if ((targetlandObj.LandData.Flags & (uint)ParcelFlags.RestrictPushObject) == (uint)ParcelFlags.RestrictPushObject)
                        {
                            // Need provisions for Group Owned here
                            if (m_host.OwnerID == targetlandObj.LandData.OwnerID ||
                                targetlandObj.LandData.IsGroupOwned ||
                                m_host.OwnerID == targetID)
                            {
                                pushAllowed = true;
                            }

                            //ParcelFlags.RestrictPushObject
                            //pushAllowed = true;
                        }
                        else
                        {
                            // Parcel isn't push restricted
                            pushAllowed = true;
                        }
                    }
                }
            }
            if (pushAllowed)
            {
                float distance = (PusheePos - m_host.AbsolutePosition).Length();
                float distance_term = distance * distance * distance; // Script Energy
                float pusher_mass = m_host.GetMass();

                float PUSH_ATTENUATION_DISTANCE = 17f;
                float PUSH_ATTENUATION_SCALE = 5f;
                float distance_attenuation = 1f;
                if (distance > PUSH_ATTENUATION_DISTANCE)
                {
                    float normalized_units = 1f + (distance - PUSH_ATTENUATION_DISTANCE) / PUSH_ATTENUATION_SCALE;
                    distance_attenuation = 1f / normalized_units;
                }

                Vector3 applied_linear_impulse = new Vector3((float)impulse.x, (float)impulse.y, (float)impulse.z);
                {
                    float impulse_length = applied_linear_impulse.Length();

                    float desired_energy = impulse_length * pusher_mass;
                    if (desired_energy > 0f)
                        desired_energy += distance_term;

                    float scaling_factor = 1f;
                    scaling_factor *= distance_attenuation;
                    applied_linear_impulse *= scaling_factor;

                }
                if (pusheeIsAvatar)
                {
                    if (pusheeav != null)
                    {
                        if (pusheeav.PhysicsActor != null)
                        {
                            if (local != 0)
                            {
                                applied_linear_impulse *= m_host.GetWorldRotation();
                            }
                            pusheeav.PhysicsActor.AddForce(applied_linear_impulse, true);
                        }
                    }
                }
                else
                {
                    if (pusheeob != null)
                    {
                        if (pusheeob.PhysActor != null)
                        {
                            pusheeob.ApplyImpulse(applied_linear_impulse, local != 0);
                        }
                    }
                }
            }
        }
Ejemplo n.º 24
0
 public void llApplyImpulse(LSL_Vector force, int local)
 {
     m_host.AddScriptLPS(1);
     //No energy force yet
     Vector3 v = new Vector3((float)force.x, (float)force.y, (float)force.z);
     if (v.Length() > 20000.0f)
     {
         v.Normalize();
         v = v * 20000.0f;
     }
     m_host.ApplyImpulse(v, local != 0);
 }
Ejemplo n.º 25
0
        public LSL_List llCastRay(LSL_Vector start, LSL_Vector end, LSL_List options)
        {
            m_host.AddScriptLPS(1);

            Vector3 dir = new Vector3((float)(end-start).x, (float)(end-start).y, (float)(end-start).z);
            Vector3 startvector = new Vector3((float)start.x, (float)start.y, (float)start.z);
            Vector3 endvector = new Vector3((float)end.x, (float)end.y, (float)end.z);

            int count = 0;
//            int detectPhantom = 0;
            int dataFlags = 0;
            int rejectTypes = 0;

            for (int i = 0; i < options.Length; i += 2)
            {
                if (options.GetLSLIntegerItem(i) == ScriptBaseClass.RC_MAX_HITS)
                {
                    count = options.GetLSLIntegerItem(i + 1);
                }
//                else if (options.GetLSLIntegerItem(i) == ScriptBaseClass.RC_DETECT_PHANTOM)
//                {
//                    detectPhantom = options.GetLSLIntegerItem(i + 1);
//                }
                else if (options.GetLSLIntegerItem(i) == ScriptBaseClass.RC_DATA_FLAGS)
                {
                    dataFlags = options.GetLSLIntegerItem(i + 1);
                }
                else if (options.GetLSLIntegerItem(i) == ScriptBaseClass.RC_REJECT_TYPES)
                {
                    rejectTypes = options.GetLSLIntegerItem(i + 1);
                }
            }

            LSL_List list = new LSL_List();
            List<ContactResult> results = World.PhysicsScene.RaycastWorld(startvector, dir, dir.Length(), count);

            double distance = Util.GetDistanceTo(startvector, endvector);

            if (distance == 0)
                distance = 0.001;

            Vector3 posToCheck = startvector;
            ITerrainChannel channel = World.RequestModuleInterface<ITerrainChannel>();

            bool checkTerrain = !((rejectTypes & ScriptBaseClass.RC_REJECT_LAND) == ScriptBaseClass.RC_REJECT_LAND);
            bool checkAgents = !((rejectTypes & ScriptBaseClass.RC_REJECT_AGENTS) == ScriptBaseClass.RC_REJECT_AGENTS);
            bool checkNonPhysical = !((rejectTypes & ScriptBaseClass.RC_REJECT_NONPHYSICAL) == ScriptBaseClass.RC_REJECT_NONPHYSICAL);
            bool checkPhysical = !((rejectTypes & ScriptBaseClass.RC_REJECT_PHYSICAL) == ScriptBaseClass.RC_REJECT_PHYSICAL);

            for (float i = 0; i <= distance; i += 0.1f)
            {
                posToCheck = startvector  + (dir * (i / (float)distance));

                if (checkTerrain && channel[(int)(posToCheck.X + startvector.X), (int)(posToCheck.Y + startvector.Y)] < posToCheck.Z)
                {
                    ContactResult result = new ContactResult();
                    result.ConsumerID = 0;
                    result.Depth = 0;
                    result.Normal = Vector3.Zero;
                    result.Pos = posToCheck;
                    results.Add(result);
                    checkTerrain = false;
                }

                if (checkAgents)
                {
                    World.ForEachRootScenePresence(delegate(ScenePresence sp)
                    {
                        if (sp.AbsolutePosition.ApproxEquals(posToCheck, sp.PhysicsActor.Size.X))
                        {
                            ContactResult result = new ContactResult ();
                            result.ConsumerID = sp.LocalId;
                            result.Depth = 0;
                            result.Normal = Vector3.Zero;
                            result.Pos = posToCheck;
                            results.Add(result);
                        }
                    });
                }
            }

            int refcount = 0;
            foreach (ContactResult result in results)
            {
                if ((rejectTypes & ScriptBaseClass.RC_REJECT_LAND)
                    == ScriptBaseClass.RC_REJECT_LAND && result.ConsumerID == 0)
                    continue;

                ISceneEntity entity = World.GetSceneObjectPart(result.ConsumerID);

                if (entity == null && (rejectTypes & ScriptBaseClass.RC_REJECT_AGENTS) != ScriptBaseClass.RC_REJECT_AGENTS)
                    entity = World.GetScenePresence(result.ConsumerID); //Only check if we should be looking for agents

                if (entity == null)
                {
                    list.Add(UUID.Zero);

                    if ((dataFlags & ScriptBaseClass.RC_GET_LINK_NUM) == ScriptBaseClass.RC_GET_LINK_NUM)
                        list.Add(0);

                    list.Add(result.Pos);

                    if ((dataFlags & ScriptBaseClass.RC_GET_NORMAL) == ScriptBaseClass.RC_GET_NORMAL)
                        list.Add(result.Normal);

                    continue; //Can't find it, so add UUID.Zero
                }

                /*if (detectPhantom == 0 && intersection.obj is ISceneChildEntity &&
                    ((ISceneChildEntity)intersection.obj).PhysActor == null)
                    continue;*/ //Can't do this ATM, physics engine knows only of non phantom objects

                if (entity is SceneObjectPart)
                {
                    PhysicsActor pa = ((SceneObjectPart)entity).PhysActor;

                    if (pa != null && pa.IsPhysical)
                    {
                        if (!checkPhysical)
                            continue;
                    }
                    else
                    {
                        if (!checkNonPhysical)
                            continue;
                    }
                }

                refcount++;
                if ((dataFlags & ScriptBaseClass.RC_GET_ROOT_KEY) == ScriptBaseClass.RC_GET_ROOT_KEY && entity is SceneObjectPart)
                    list.Add(((SceneObjectPart)entity).ParentGroup.UUID);
                else
                    list.Add(entity.UUID);

                if ((dataFlags & ScriptBaseClass.RC_GET_LINK_NUM) == ScriptBaseClass.RC_GET_LINK_NUM)
                {
                    if (entity is SceneObjectPart)
                        list.Add(((SceneObjectPart)entity).LinkNum);
                    else
                        list.Add(0);
                }

                list.Add(result.Pos);

                if ((dataFlags & ScriptBaseClass.RC_GET_NORMAL) == ScriptBaseClass.RC_GET_NORMAL)
                    list.Add(result.Normal);
            }

            list.Add(refcount); //The status code, either the # of contacts, RCERR_SIM_PERF_LOW, or RCERR_CAST_TIME_EXCEEDED

            return list;
        }
        private void SendSitResponse(IClientAPI remoteClient, UUID targetID, Vector3 offset, Quaternion pSitOrientation)
        {
            bool autopilot = true;
            Vector3 pos = new Vector3();
            Quaternion sitOrientation = pSitOrientation;
            Vector3 cameraEyeOffset = Vector3.Zero;
            Vector3 cameraAtOffset = Vector3.Zero;

            ISceneChildEntity part = FindNextAvailableSitTarget(targetID);
            if (part.SitTargetAvatar.Count > 0)
                part = FindNextAvailableSitTarget(targetID, part.UUID);

            m_requestedSitTargetUUID = part.UUID;
            m_sitting = true;

            // Is a sit target available?
            Vector3 avSitOffSet = part.SitTargetPosition;
            Quaternion avSitOrientation = part.SitTargetOrientation;
            bool UseSitTarget = false;

            bool SitTargetisSet =
                (!(avSitOffSet.X == 0f && avSitOffSet.Y == 0f && avSitOffSet.Z == 0f &&
                   (
                       avSitOrientation.X == 0f && avSitOrientation.Y == 0f && avSitOrientation.Z == 0f &&
                       avSitOrientation.W == 1f // Valid Zero Rotation quaternion
                       ||
                       avSitOrientation.X == 0f && avSitOrientation.Y == 0f && avSitOrientation.Z == 1f &&
                       avSitOrientation.W == 0f // W-Z Mapping was invalid at one point
                       ||
                       avSitOrientation.X == 0f && avSitOrientation.Y == 0f && avSitOrientation.Z == 0f &&
                       avSitOrientation.W == 0f // Invalid Quaternion
                   )
                  ));

            m_requestedSitTargetUUID = part.UUID;
            m_sitting = true;
            part.SetAvatarOnSitTarget(UUID);
            if (SitTargetisSet)
            {
                offset = new Vector3(avSitOffSet.X, avSitOffSet.Y, avSitOffSet.Z);
                sitOrientation = avSitOrientation;
                autopilot = false;
                UseSitTarget = true;
            }

            pos = part.AbsolutePosition; // +offset;
            if (m_physicsActor != null)
            {
                // If we're not using the client autopilot, we're immediately warping the avatar to the location
                // We can remove the physicsActor until they stand up.
                m_sitAvatarHeight = m_physicsActor.Size.Z;

                if (autopilot)
                {
                    Vector3 targetpos = new Vector3(m_pos.X - part.AbsolutePosition.X - (part.Scale.X/2),
                                                    m_pos.Y - part.AbsolutePosition.Y - (part.Scale.Y/2),
                                                    m_pos.Z - part.AbsolutePosition.Z - (part.Scale.Z/2));
                    if (targetpos.Length() < 4.5)
                    {
                        autopilot = false;
                        Velocity = Vector3.Zero;
                        RemoveFromPhysicalScene();
                        Vector3 Position = part.AbsolutePosition;
                        Vector3 MovePos = Vector3.Zero;
                        IAvatarAppearanceModule appearance = RequestModuleInterface<IAvatarAppearanceModule>();
                        if (appearance != null)
                        {
                            switch (part.GetPrimType())
                            {
                                case PrimType.SCULPT:
                                case PrimType.PRISM:
                                case PrimType.RING:
                                case PrimType.TUBE:
                                case PrimType.TORUS:
                                case PrimType.CYLINDER:
                                case PrimType.BOX:
                                    Position.Z += part.Scale.Z / 2f;
                                    Position.Z += appearance.Appearance.AvatarHeight / 2;
                                    Position.Z -= (float)(SIT_TARGET_ADJUSTMENT.Z / 1.5);
                                    //m_appearance.AvatarHeight / 15;
                                    MovePos.X = (part.Scale.X / 2) + .1f;
                                    MovePos *= Rotation;
                                    break;
                                case PrimType.SPHERE:
                                    Position.Z += part.Scale.Z / 2f;
                                    Position.Z += appearance.Appearance.AvatarHeight / 2;
                                    Position.Z -= (float)(SIT_TARGET_ADJUSTMENT.Z / 1.5);
                                    //m_appearance.AvatarHeight / 15;
                                    MovePos.X = (float)(part.Scale.X / 2.5);
                                    MovePos *= Rotation;
                                    break;
                            }
                        }
                        Position += MovePos;
                        AbsolutePosition = Position;
                    }
                }
                else
                    RemoveFromPhysicalScene();
            }

            cameraAtOffset = part.CameraAtOffset;
            cameraEyeOffset = part.CameraEyeOffset;
            bool forceMouselook = part.ForceMouselook;

            ControllingClient.SendSitResponse(part.UUID, offset, sitOrientation, autopilot, cameraAtOffset,
                                              cameraEyeOffset, forceMouselook);
            //Remove any bad terse updates lieing around
            foreach (IScenePresence sp in Scene.GetScenePresences())
                sp.SceneViewer.ClearPresenceUpdates(this);
            System.Threading.Thread.Sleep(10);
            //Sleep for a little bit to make sure all other threads are finished sending anything
            // This calls HandleAgentSit twice, once from here, and the client calls
            // HandleAgentSit itself after it gets to the location
            // It doesn't get to the location until we've moved them there though
            // which happens in HandleAgentSit :P
            m_autopilotMoving = autopilot;
            m_autoPilotTarget = pos;
            m_sitAtAutoTarget = autopilot;
            if (!autopilot)
                HandleAgentSit(remoteClient, UUID, String.IsNullOrEmpty(m_nextSitAnimation) ? "SIT" : m_nextSitAnimation,
                               UseSitTarget);
        }
Ejemplo n.º 27
0
        /// <summary>
        /// Calculate the sun's orbital position and its velocity.
        /// </summary>
        private void GenSunPos()
        {
            // Time in seconds since UTC to use to calculate sun position.
            PosTime = CurrentTime;

            if (m_SunFixed)
            {
                // SunFixedHour represents the "hour of day" we would like
                // It's represented in 24hr time, with 0 hour being sun-rise
                // Because our day length is probably not 24hrs {LL is 6} we need to do a bit of math

                // Determine the current "day" from current time, so we can use "today"
                // to determine Seasonal Tilt and what'not

                // Integer math rounded is on purpose to drop fractional day, determines number 
                // of virtual days since Epoch
                PosTime = CurrentTime / SecondsPerSunCycle;

                // Since we want number of seconds since Epoch, multiply back up
                PosTime *= SecondsPerSunCycle;

                // Then offset by the current Fixed Sun Hour
                // Fixed Sun Hour needs to be scaled to reflect the user configured Seconds Per Sun Cycle
                PosTime += (ulong)((m_SunFixedHour / 24.0) * (ulong)SecondsPerSunCycle);
            }
            else
            {
                if (m_DayTimeSunHourScale != 0.5f)
                {
                    ulong CurDaySeconds = CurrentTime % SecondsPerSunCycle;
                    double CurDayPercentage = (double)CurDaySeconds / SecondsPerSunCycle;

                    ulong DayLightSeconds = (ulong)(m_DayTimeSunHourScale * SecondsPerSunCycle);
                    ulong NightSeconds = SecondsPerSunCycle - DayLightSeconds;

                    PosTime = CurrentTime / SecondsPerSunCycle;
                    PosTime *= SecondsPerSunCycle;

                    if (CurDayPercentage < 0.5)
                    {
                        PosTime += (ulong)((CurDayPercentage / .5) * DayLightSeconds);
                    }
                    else
                    {
                        PosTime += DayLightSeconds;
                        PosTime += (ulong)(((CurDayPercentage - 0.5) / .5) * NightSeconds);
                    }
                }

            }

            TotalDistanceTravelled = SunSpeed * PosTime;  // distance measured in radians

            OrbitalPosition = (float)(TotalDistanceTravelled % m_SunCycle); // position measured in radians

            // TotalDistanceTravelled += HoursToRadians-(0.25*Math.PI)*Math.Cos(HoursToRadians)-OrbitalPosition;
            // OrbitalPosition         = (float) (TotalDistanceTravelled%SunCycle);

            SeasonalOffset = SeasonSpeed * PosTime; // Present season determined as total radians travelled around season cycle
            Tilt.W = (float)(m_AverageTilt + (m_SeasonalTilt * Math.Sin(SeasonalOffset))); // Calculate seasonal orbital N/S tilt

            // m_log.Debug("[SUN] Total distance travelled = "+TotalDistanceTravelled+", present position = "+OrbitalPosition+".");
            // m_log.Debug("[SUN] Total seasonal progress = "+SeasonalOffset+", present tilt = "+Tilt.W+".");

            // The sun rotates about the Z axis

            Position.X = (float)Math.Cos(-TotalDistanceTravelled);
            Position.Y = (float)Math.Sin(-TotalDistanceTravelled);
            Position.Z = 0;

            // For interest we rotate it slightly about the X access.
            // Celestial tilt is a value that ranges .025

            Position *= Tilt;

            // Finally we shift the axis so that more of the
            // circle is above the horizon than below. This
            // makes the nights shorter than the days.

            Position = Vector3.Normalize(Position);
            Position.Z = Position.Z + (float)HorizonShift;
            Position = Vector3.Normalize(Position);

            // m_log.Debug("[SUN] Position("+Position.X+","+Position.Y+","+Position.Z+")");

            Velocity.X = 0;
            Velocity.Y = 0;
            Velocity.Z = (float)SunSpeed;

            // Correct angular velocity to reflect the seasonal rotation

            Magnitude = Position.Length();
            if (m_SunFixed)
            {
                Velocity.X = 0;
                Velocity.Y = 0;
                Velocity.Z = 0;

            }
            else
            {
                Velocity = (Velocity * Tilt) * (1.0f / Magnitude);
            }

            // TODO: Decouple this, so we can get rid of Linden Hour info
            // Update Region infor with new Sun Position and Hour
            // set estate settings for region access to sun position
            if (receivedEstateToolsSunUpdate)
            {
                m_scene.RegionInfo.RegionSettings.SunVector = Position;
                m_scene.RegionInfo.RegionSettings.SunPosition = GetCurrentTimeAsLindenSunHour();
            }
        }
Ejemplo n.º 28
0
        private static MyCharacter CreateCharacterBase(MatrixD worldMatrix, ref Vector3 velocity, string characterName, string model, Vector3? colorMask, bool AIMode, bool useInventory = true, MyBotDefinition botDefinition = null)
        {
            MyCharacter character = new MyCharacter();
            MyObjectBuilder_Character objectBuilder = MyCharacter.Random();
            objectBuilder.CharacterModel = model ?? objectBuilder.CharacterModel;

            if (colorMask.HasValue)
                objectBuilder.ColorMaskHSV = colorMask.Value;

            objectBuilder.JetpackEnabled = MySession.Static.CreativeMode;
            objectBuilder.Battery = new MyObjectBuilder_Battery { CurrentCapacity = 1 };
            objectBuilder.AIMode = AIMode;
            objectBuilder.DisplayName = characterName;
            objectBuilder.LinearVelocity = velocity;
            objectBuilder.PositionAndOrientation = new MyPositionAndOrientation(worldMatrix);
            character.Init(objectBuilder);

            MyEntities.RaiseEntityCreated(character);
            MyEntities.Add(character);

            System.Diagnostics.Debug.Assert(character.GetInventory() as MyInventory != null, "Null or unexpected inventory type returned!");
            if (useInventory)
                MyWorldGenerator.InitInventoryWithDefaults(character.GetInventory() as MyInventory);
            else if (botDefinition != null)
            {
                // use inventory from bot definition
                botDefinition.AddItems(character);
            }
            //character.PositionComp.SetWorldMatrix(worldMatrix);
            if (velocity.Length() > 0)
            {
                var jetpack = character.JetpackComp;

                if (jetpack != null)
                    jetpack.EnableDampeners(false, false);
            }

            return character;
        }
Ejemplo n.º 29
0
        private void UpdateStandup(ref Vector3 gravity, ref Vector3 chUp, ref Vector3 chForward)
        {
            Vector3 minusGravity = -Vector3.Normalize(gravity);
            Vector3 testUp = minusGravity;
            if (Physics != null && Physics.CharacterProxy != null && Physics.CharacterProxy.Supported)
            {
                Vector3 supportNormal = Physics.CharacterProxy.SupportNormal;
                if (Definition.RotationToSupport == MyEnumCharacterRotationToSupport.OneAxis)
                {
                    float cosAngleMinusGravityToNormal = minusGravity.Dot(ref supportNormal);
                    if (!MyUtils.IsZero(cosAngleMinusGravityToNormal - 1) &&
                        !MyUtils.IsZero(cosAngleMinusGravityToNormal + 1))
                    {
                        Vector3 poleVec = minusGravity.Cross(supportNormal);
                        poleVec.Normalize();
                        testUp = Vector3.Lerp(supportNormal, minusGravity, Math.Abs(poleVec.Dot(WorldMatrix.Forward)));
                    }
                }
                else if (Definition.RotationToSupport == MyEnumCharacterRotationToSupport.Full)
                {
                    testUp = supportNormal;
                }
            }

            var dotProd = Vector3.Dot(chUp, testUp);
            var lenProd = chUp.Length() * testUp.Length();
            var divOperation = dotProd / lenProd;

            // check-up proper division result and for NaN
            if (float.IsNaN(divOperation) || float.IsNegativeInfinity(divOperation) || float.IsPositiveInfinity(divOperation)) divOperation = 1;
            divOperation = MathHelper.Clamp(divOperation, -1.0f, 1.0f);

            const float standUpSpeedAnglePerFrame = 0.04f;

            if (!MyUtils.IsZero(divOperation - 1, 0.00001f))
            {
                float angle = 0;
                //if direct opposite
                if (MyUtils.IsZero(divOperation + 1, 0.00001f))
                    angle = 0.1f;
                else
                    angle = (float)(Math.Acos(divOperation));

                float relativeSpeed = Sync.IsServer ? 1.0f : Sync.ServerSimulationRatio;
                angle = relativeSpeed * System.Math.Min(Math.Abs(angle), standUpSpeedAnglePerFrame) * Math.Sign(angle);

                Vector3 normal = Vector3.Cross(chUp, testUp);

                if (normal.LengthSquared() > 0)
                {
                    normal = Vector3.Normalize(normal);
                    chUp = Vector3.TransformNormal(chUp, Matrix.CreateFromAxisAngle(normal, angle));
                    chForward = Vector3.TransformNormal(chForward, Matrix.CreateFromAxisAngle(normal, angle));
                }
            }
        }
Ejemplo n.º 30
0
        // end Step
        void MoveLinear(float pTimestep, ODEPhysicsScene _pParentScene, ODEPrim parent)
        {
            bool ishovering = false;
            bool bypass_buoyancy = false;
            d.Vector3 dpos = d.BodyGetPosition(Body);
            d.Vector3 dvel_now = d.BodyGetLinearVel(Body);
            d.Quaternion drotq_now = d.BodyGetQuaternion(Body);

            Vector3 pos = new Vector3(dpos.X, dpos.Y, dpos.Z);
            Vector3 vel_now = new Vector3(dvel_now.X, dvel_now.Y, dvel_now.Z);
            Quaternion rotq = new Quaternion(drotq_now.X, drotq_now.Y, drotq_now.Z, drotq_now.W);
            rotq *= m_referenceFrame; //add reference rotation to rotq
            Quaternion irotq = new Quaternion(-rotq.X, -rotq.Y, -rotq.Z, rotq.W);

            m_newVelocity = Vector3.Zero;

            if (!(m_lastPositionVector.X == 0 &&
                  m_lastPositionVector.Y == 0 &&
                  m_lastPositionVector.Z == 0))
            {
                ///Only do this if we have a last position
                m_lastposChange.X = pos.X - m_lastPositionVector.X;
                m_lastposChange.Y = pos.Y - m_lastPositionVector.Y;
                m_lastposChange.Z = pos.Z - m_lastPositionVector.Z;
            }

            #region Blocking Change

            if (m_BlockingEndPoint != Vector3.Zero)
            {
                bool needUpdateBody = false;
                if(pos.X >= (m_BlockingEndPoint.X - 1)) {
                    pos.X -= m_lastposChange.X + 1;
                    needUpdateBody = true;
                }
                if(pos.Y >= (m_BlockingEndPoint.Y - 1)) {
                    pos.Y -= m_lastposChange.Y + 1;
                    needUpdateBody = true;
                }
                if(pos.Z >= (m_BlockingEndPoint.Z - 1)) {
                    pos.Z -= m_lastposChange.Z + 1;
                    needUpdateBody = true;
                }
                if(pos.X <= 0) {
                    pos.X += m_lastposChange.X + 1;
                    needUpdateBody = true;
                }
                if(pos.Y <= 0) {
                    pos.Y += m_lastposChange.Y + 1;
                    needUpdateBody = true;
                }
                if(needUpdateBody)
                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
            }

            #endregion

            #region Terrain checks

            float terrainHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
            if(pos.Z < terrainHeight - 5) {
                pos.Z = terrainHeight + 2;
                m_lastPositionVector = pos;
                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
            }
            else if(pos.Z < terrainHeight) {
                m_newVelocity.Z += 1;
            }

            #endregion

            #region Hover

            Vector3 hovervel = Vector3.Zero;
            if(m_VhoverTimescale * pTimestep <= 300.0f && m_VhoverHeight > 0.0f) {
                ishovering = true;

                if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) {
                    m_VhoverTargetHeight = (float) _pParentScene.GetWaterLevel(pos.X, pos.Y) + 0.3f + m_VhoverHeight;
                }
                else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) {
                    m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
                }
                else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) {
                    m_VhoverTargetHeight = m_VhoverHeight;
                }
                else {
                    float waterlevel = (float)_pParentScene.GetWaterLevel(pos.X, pos.Y) + 0.3f;
                    float terrainlevel = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
                    if(waterlevel > terrainlevel) {
                        m_VhoverTargetHeight = waterlevel + m_VhoverHeight;
                    }
                    else {
                        m_VhoverTargetHeight = terrainlevel + m_VhoverHeight;
                    }
                }

                float tempHoverHeight = m_VhoverTargetHeight;
                if((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0) {
                    // If body is aready heigher, use its height as target height
                    if (pos.Z > tempHoverHeight) {
                        tempHoverHeight = pos.Z;
                        bypass_buoyancy = true; //emulate sl bug
                    }
                }
                if((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) {
                    if((pos.Z - tempHoverHeight) > .2 || (pos.Z - tempHoverHeight) < -.2) {
                        float h = tempHoverHeight;
                        float groundHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
                        if(groundHeight >= tempHoverHeight)
                            h = groundHeight;

                        d.BodySetPosition(Body, pos.X, pos.Y, h);
                    }
                }
                else {
                    hovervel.Z -= ((dvel_now.Z * 0.1f * m_VhoverEfficiency) + (pos.Z - tempHoverHeight)) / m_VhoverTimescale;
                    hovervel.Z *= 7.0f * (1.0f + m_VhoverEfficiency);

                    if(hovervel.Z > 50.0f) hovervel.Z = 50.0f;
                    if(hovervel.Z < -50.0f) hovervel.Z = -50.0f;
                }
            }

            #endregion

            #region limitations

            //limit maximum velocity
            if(vel_now.LengthSquared() > 1e6f) {
                vel_now /= vel_now.Length();
                vel_now *= 1000f;
                d.BodySetLinearVel(Body, vel_now.X, vel_now.Y, vel_now.Z);
            }

            //block movement in x and y when low velocity
            bool enable_ode_gravity = true;
            if(vel_now.LengthSquared() < 0.02f) {
                d.BodySetLinearVel(Body, 0.0f, 0.0f, 0.0f);
                vel_now = Vector3.Zero;
                if(parent.LinkSetIsColliding)
                    enable_ode_gravity = false;
            }

            #endregion

            #region Linear motors

            //cancel directions of linear friction for certain vehicles without having effect on ode gravity
            Vector3 vt_vel_now = vel_now;
            bool no_grav_calc = false;
            if((Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON) && m_VehicleBuoyancy != 1.0f) {
                vt_vel_now.Z = 0.0f;
                no_grav_calc = true;
            }

            if(!bypass_buoyancy) {
                //apply additional gravity force over ode gravity
                if(m_VehicleBuoyancy == 1.0f) enable_ode_gravity = false;
                else if(m_VehicleBuoyancy != 0.0f && enable_ode_gravity) {
                    float grav = _pParentScene.gravityz * parent.GravityMultiplier * -m_VehicleBuoyancy;
                    m_newVelocity.Z += grav * Mass;
                }
            }

            //set ode gravity
            d.BodySetGravityMode(Body, enable_ode_gravity);

            //add default linear friction (mimic sl friction as much as possible)
            float initialFriction = 0.055f;
            float defaultFriction = 180f;

            Vector3 friction = Vector3.Zero;
            if(parent.LinkSetIsColliding || ishovering) {
                if(vt_vel_now.X > 0.0f) friction.X += initialFriction;
                if(vt_vel_now.Y > 0.0f) friction.Y += initialFriction;
                if(vt_vel_now.Z > 0.0f) friction.Z += initialFriction;
                if(vt_vel_now.X < 0.0f) friction.X -= initialFriction;
                if(vt_vel_now.Y < 0.0f) friction.Y -= initialFriction;
                if(vt_vel_now.Z < 0.0f) friction.Z -= initialFriction;
                friction += vt_vel_now / defaultFriction;
                friction *= irotq;
            }

            //world -> body orientation
            vel_now *= irotq;
            vt_vel_now *= irotq;

            //add linear friction
            if(vt_vel_now.X > 0.0f)
                friction.X += vt_vel_now.X * vt_vel_now.X / m_linearFrictionTimescale.X;
            else
                friction.X -= vt_vel_now.X * vt_vel_now.X / m_linearFrictionTimescale.X;

            if(vt_vel_now.Y > 0.0f)
                friction.Y += vt_vel_now.Y * vt_vel_now.Y / m_linearFrictionTimescale.Y;
            else
                friction.Y -= vt_vel_now.Y * vt_vel_now.Y / m_linearFrictionTimescale.Y;

            if(vt_vel_now.Z > 0.0f)
                friction.Z += vt_vel_now.Z * vt_vel_now.Z / m_linearFrictionTimescale.Z;
            else
                friction.Z -= vt_vel_now.Z * vt_vel_now.Z / m_linearFrictionTimescale.Z;

            friction /= 1.35f; //1.5f;

            //add linear forces
            //not the best solution, but it is really close to sl motor velocity, and just works
            Vector3 motorVelocity = (m_linearMotorDirection * 3.0f - vel_now) / m_linearMotorTimescale / 5.0f;  //2.8f;
            Vector3 motorfrictionamp = new Vector3(4.0f, 4.0f, 4.0f);
            Vector3 motorfrictionstart = new Vector3(1.0f, 1.0f, 1.0f);
            motorVelocity *= motorfrictionstart + motorfrictionamp / (m_linearFrictionTimescale * pTimestep);
            float addVel = 0.15f;
            if(motorVelocity.LengthSquared() > 0.01f) {
                if(motorVelocity.X > 0.0f) motorVelocity.X += addVel;
                if(motorVelocity.Y > 0.0f) motorVelocity.Y += addVel;
                if(motorVelocity.Z > 0.0f) motorVelocity.Z += addVel;
                if(motorVelocity.X < 0.0f) motorVelocity.X -= addVel;
                if(motorVelocity.Y < 0.0f) motorVelocity.Y -= addVel;
                if(motorVelocity.Z < 0.0f) motorVelocity.Z -= addVel;
            }

            //free run
            if(vel_now.X > m_linearMotorDirection.X && m_linearMotorDirection.X >= 0.0f) motorVelocity.X = 0.0f;
            if(vel_now.Y > m_linearMotorDirection.Y && m_linearMotorDirection.Y >= 0.0f) motorVelocity.Y = 0.0f;
            if(vel_now.Z > m_linearMotorDirection.Z && m_linearMotorDirection.Z >= 0.0f) motorVelocity.Z = 0.0f;

            if(vel_now.X < m_linearMotorDirection.X && m_linearMotorDirection.X <= 0.0f) motorVelocity.X = 0.0f;
            if(vel_now.Y < m_linearMotorDirection.Y && m_linearMotorDirection.Y <= 0.0f) motorVelocity.Y = 0.0f;
            if(vel_now.Z < m_linearMotorDirection.Z && m_linearMotorDirection.Z <= 0.0f) motorVelocity.Z = 0.0f;

            //decay linear motor
            m_linearMotorDirection *= (1.0f - 1.0f/m_linearMotorDecayTimescale);

            #endregion

            #region Deflection

            //does only deflect on x axis from world orientation with z axis rotated to body
            //it is easier to filter out gravity deflection for vehicles(car) without rotation problems
            Quaternion irotq_z = irotq;
            irotq_z.X = 0.0f;
            irotq_z.Y = 0.0f;
            float mag = (float)Math.Sqrt(irotq_z.W * irotq_z.W + irotq_z.Z * irotq_z.Z); //normalize
            irotq_z.W /= mag;
            irotq_z.Z /= mag;

            Vector3 vel_defl = new Vector3(dvel_now.X, dvel_now.Y, dvel_now.Z);
            vel_defl *= irotq_z;
            if(no_grav_calc) {
                vel_defl.Z = 0.0f;
                if(!parent.LinkSetIsColliding) vel_defl.Y = 0.0f;
            }

            Vector3 deflection = vel_defl / m_linearDeflectionTimescale * m_linearDeflectionEfficiency * 100.0f;

            float deflectionLengthY = Math.Abs(deflection.Y);
            float deflectionLengthX = Math.Abs(deflection.X);

            deflection.Z = 0.0f;
            if((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) == 0) {
                deflection.Z = deflectionLengthX;
                deflection.X = -deflection.X;
            }

            if(vel_defl.X < 0.0f) deflection.X = -deflectionLengthY;
            else if(vel_defl.X >= 0.0f) deflection.X = deflectionLengthY;
            deflection.Y = -deflection.Y;

            irotq_z.W = -irotq_z.W;
            deflection *= irotq_z;

            #endregion

            #region Deal with tainted forces

            Vector3 TaintedForce = new Vector3();
            if(m_forcelist.Count != 0) {
                try {
                    TaintedForce = m_forcelist.Aggregate(TaintedForce, (current, t) => current + (t));
                }
                catch(IndexOutOfRangeException) {
                    TaintedForce = Vector3.Zero;
                }
                catch(ArgumentOutOfRangeException) {
                    TaintedForce = Vector3.Zero;
                }
                m_forcelist = new List<Vector3>();
            }

            #endregion

            #region Add Forces

            //add forces
            m_newVelocity -= (friction *= Mass / pTimestep);
            m_newVelocity += TaintedForce;
            motorVelocity *= Mass / pTimestep;

            #endregion

            #region No X,Y,Z

            if((m_flags & (VehicleFlag.NO_X)) != 0)
                m_newVelocity.X = -vel_now.X * Mass / pTimestep;
            if((m_flags & (VehicleFlag.NO_Y)) != 0)
                m_newVelocity.Y = -vel_now.Y * Mass / pTimestep;
            if((m_flags & (VehicleFlag.NO_Z)) != 0)
                m_newVelocity.Z = -vel_now.Z * Mass / pTimestep;

            #endregion

            m_newVelocity *= rotq;
            m_newVelocity += (hovervel *= Mass / pTimestep);

            if(parent.LinkSetIsColliding || Type == Vehicle.TYPE_AIRPLANE || Type == Vehicle.TYPE_BALLOON || ishovering) {
                m_newVelocity += deflection;

                motorVelocity *= rotq;

                if((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0 && motorVelocity.Z > 0.0f) motorVelocity.Z = 0.0f;
                m_newVelocity += motorVelocity;
            }

            d.BodyAddForce(Body, m_newVelocity.X, m_newVelocity.Y, m_newVelocity.Z);
        }