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); }
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(); }
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))); }
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); }
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); }
/// <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; }
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); }
public MyVoxelMapImpostor(Vector3 position, float radius, float angle) { Position = position; Radius = radius; Angle = angle; m_distance = position.Length(); }
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(); }
/// <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; }
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); }
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; }
/// <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 ); }
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; }
/// <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; }
// 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); } }
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; }
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; }
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; }
public double llVecMag(IScriptInstance script, Vector3 vec) { return vec.Length(); }
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); } } } } }
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); }
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); }
/// <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(); } }
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; }
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)); } } }
// 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); }