Ejemplo n.º 1
0
        /// public メソッド
        ///---------------------------------------------------------------------------
        /// カプセルと三角形との衝突
        public bool Check( DemoGame.GeometryCapsule moveCap, ShapeTriangles trgShape )
        {
            Vector3 collPos = StaticDataList.getVectorZero();
            calMovePos = moveCap.EndPos;
            calBestDis = -1.0f;
            calBestId  = -1;

            float checDis = moveCap.Line.Length + moveCap.R + collCheckDis;

            if( AppDebug.CollLightFlg == false ){
            for( int i=0; i<trgShape.EntryNum; i++ ){

                float a = (calMovePos.Dot( trgShape.Triangle[i].Plane.Nor ) + trgShape.Triangle[i].Plane.D);
                if( a >= checDis || a <= -checDis ){
                    continue;
                }

                if( DemoGame.CommonCollision.CheckSphereAndTriangle( moveCap, trgShape.Triangle[i], ref collPos ) == true ){
                    float dis = Common.VectorUtil.Distance( collPos, moveCap.StartPos );
                    if( dis < calBestDis || calBestId < 0 ){
                        calMovePos        = collPos;
                        calBestDis        = dis;
                        calBestId        = i;
                    }
                }
                AppDebug.CollCnt ++;
            }
            }
            else{
            for( int i=0; i<trgShape.EntryNum; i++ ){

                float a = (calMovePos.Dot( trgShape.Triangle[i].Plane.Nor ) + trgShape.Triangle[i].Plane.D);
                if( a >= checDis || a <= -checDis ){
                    continue;
                }

                if( DemoGame.CommonCollision.CheckLineAndTriangle( moveCap.Line, trgShape.Triangle[i], ref collPos ) == true ){
                    float dis = Common.VectorUtil.Distance( collPos, moveCap.StartPos );
                    if( dis < calBestDis || calBestId < 0 ){
                        calMovePos        = collPos;
                        calBestDis        = dis;
                        calBestId        = i;
                    }
                }
                AppDebug.CollCnt ++;
            }
            }

            if( calBestId >= 0 ){
            return true;
            }
            return false;
        }
        public void AnyOrthogonalShouldWork()
        {

            var r = new Random();
            for (int i = 0; i < 1000; i++)
            {
                var v = new Vector3((double) r.NextDouble(), (double) r.NextDouble(), (double) r.NextDouble());
                var o = v.Orthogonal();
                v.Dot(o).Should().BeApproximately(0, 1e-9);

            }
            
        }
Ejemplo n.º 3
0
        /// 最近接距離を求める
        public float CheckNearDis( Vector3 pos )
        {
            float isDis = 0.0f;
            float bestDis = 0.0f;

            /// 面単位で確認すればいいので1つ飛ばしで判定
            for( int i=0; i<EntryNum; i+=2 ){
            isDis = pos.Dot( Triangle[i].Plane.Nor ) + Triangle[i].Plane.D;
            if( i == 0 || isDis > bestDis ){
                bestDis = isDis;
            }
            }
            return bestDis;
        }
Ejemplo n.º 4
0
        /// 最近接距離を求める
        public float CheckNearDis( Vector3 pos )
        {
            float isDis = 0.0f;
            float bestDis = 0.0f;

            /// 上面
            bestDis = pos.Dot( Triangle[0].Plane.Nor ) + Triangle[0].Plane.D;

            /// 下面
            bestDis = pos.Dot( Triangle[1].Plane.Nor ) + Triangle[1].Plane.D;
            if( isDis > bestDis ){
            bestDis = isDis;
            }

            /// 右側面
            bestDis = pos.Dot( Triangle[EntryNum-4].Plane.Nor ) + Triangle[EntryNum-4].Plane.D;
            if( isDis > bestDis ){
            bestDis = isDis;
            }

            /// 左側面
            bestDis = pos.Dot( Triangle[EntryNum-2].Plane.Nor ) + Triangle[EntryNum-2].Plane.D;
            if( isDis > bestDis ){
            bestDis = isDis;
            }

            /// 遠面
            for( int i=0; i<objDivisionNum; i++ ){
            isDis = pos.Dot( Triangle[i*4+2].Plane.Nor ) + Triangle[i*4+2].Plane.D;
            if( i == 0 || isDis > bestDis ){
                bestDis = isDis;
            }
            }

            return bestDis;
        }
Ejemplo n.º 5
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.º 6
0
 /// <summary>
 ///   <para>Reflects a vector off the plane defined by a normal.</para>
 /// </summary>
 /// <param name="inDirection"></param>
 /// <param name="inNormal"></param>
 public static Vector3<byte> Reflect(this Vector3<byte> inDirection, Vector3<byte> inNormal)
 {
     //In case of bytes it is -2 * some byte (which is byte). Dot product is closed under bytes so this is valid for bytes.
     byte twoTimesDotNDir = (byte)(-2 * (int)inNormal.Dot(inDirection));
     //this operation is also closed for bytes. We do not need to make an extension method.
     return (twoTimesDotNDir * inNormal) + inDirection;
 }
Ejemplo n.º 7
0
        /// <summary>
        ///   <para>Projects a vector onto another vector.</para>
        /// </summary>
        /// <param name="vector"></param>
        /// <param name="onNormal"></param>
        public static Vector3<float> Project(this Vector3<float> vector, Vector3<float> onNormal)
        {
            //return ProjectInternalGeneric(vector, onNormal);
            float single = onNormal.Dot(onNormal);

            if (single < Vector3<float>.kEpsilon)
            {
                return Vector3<float>.zero;
            }

            return (onNormal * vector.Dot(onNormal)) * (1.0f / single);
        }
Ejemplo n.º 8
0
 /// <summary>
 ///   <para>Reflects a vector off the plane defined by a normal.</para>
 /// </summary>
 /// <param name="inDirection"></param>
 /// <param name="inNormal"></param>
 public static Vector3<int> Reflect(this Vector3<int> inDirection, Vector3<int> inNormal)
 {
     //In case of ints it is -2 * some int (which is int). Dot product is closed under ints so this is valid for ints.
     int twoTimesDotNDir = -2 * inNormal.Dot(inDirection);
     //this operation is also closed for ints. We do not need to make an extension method.
     return (twoTimesDotNDir * inNormal) + inDirection;
 }
        public override void ProcessTriangle(ref Vector3 point0, ref Vector3 point1, ref Vector3 point2, int partId, int triangleIndex)
        {
            Vector3 v10 = point1 - point0;
            Vector3 v20 = point2 - point0;

            Vector3 triangleNormal = v10.Cross(v20);

            float dist;
            point0.Dot(ref triangleNormal, out dist);
            float distA = triangleNormal.Dot(From) - dist;
            float distB = triangleNormal.Dot(To) - dist;

            if (distA * distB >= 0.0f)
            {
                return; // same sign
            }

            if (((Flags & EFlags.FilterBackfaces) != 0) && (distA <= 0.0f))
            {
                // Backface, skip check
                return;
            }


            float proj_length = distA - distB;
            float distance = (distA) / (proj_length);
            // Now we have the intersection point on the plane, we'll see if it's inside the triangle
            // Add an epsilon as a tolerance for the raycast,
            // in case the ray hits exacly on the edge of the triangle.
            // It must be scaled for the triangle size.

            if (distance < HitFraction)
            {
                float edgeTolerance = triangleNormal.LengthSquared;
                edgeTolerance *= -0.0001f;
                Vector3 point = Vector3.Lerp(From, To, distance);
                {
                    Vector3 v0p; v0p = point0 - point;
                    Vector3 v1p; v1p = point1 - point;
                    Vector3 cp0; cp0 = v0p.Cross(v1p);

                    float dot;
                    cp0.Dot(ref triangleNormal, out dot);
                    if (dot >= edgeTolerance)
                    {
                        Vector3 v2p; v2p = point2 - point;
                        Vector3 cp1;
                        cp1 = v1p.Cross(v2p);
                        cp1.Dot(ref triangleNormal, out dot);
                        if (dot >= edgeTolerance)
                        {
                            Vector3 cp2;
                            cp2 = v2p.Cross(v0p);

                            cp2.Dot(ref triangleNormal, out dot);
                            if (dot >= edgeTolerance)
                            {
                                //@BP Mod
                                // Triangle normal isn't normalized
                                triangleNormal.Normalize();

                                //@BP Mod - Allow for unflipped normal when raycasting against backfaces
                                if (((Flags & EFlags.KeepUnflippedNormal) == 0) && (distA <= 0.0f))
                                {
                                    triangleNormal = -triangleNormal;
                                }
                                HitFraction = ReportHit(ref triangleNormal, distance, partId, triangleIndex);
                            }
                        }
                    }
                }
            }
        }
Ejemplo n.º 10
0
        /// <summary>
        ///   <para>Projects a vector onto another vector.</para>
        /// </summary>
        /// <param name="vector"></param>
        /// <param name="onNormal"></param>
        public static Vector3<double> Project(this Vector3<double> vector, Vector3<double> onNormal)
        {
            double single = onNormal.Dot(onNormal);

            if (single <= Vector3<double>.kEpsilon)
            {
                return Vector3<double>.zero;
            }

            return (onNormal * vector.Dot(onNormal)) * (1d / single);
        }
        public static IBody2 CreateSemiCirclularSheet
            ( this IModeler modeler
            , Vector3 center
            , Vector3 vNormal
            , Vector3 vRef // Horizontal  
            , double radius)
        {
            // Should be orthogonal
            Debug.Assert(vRef.Dot(vNormal)<1e-9);

            var math = SwAddinBase.Active.Math;
            var centerSw = center.ToSwMathPoint();
            var vNormalSw = vNormal.ToSwMathPoint();
            var vNormalOrthSw = vRef.ToSWVector(math).Normalise();

            var centerDbls = centerSw.ArrayData;
            var vNormalDbls = vNormalSw.ArrayData;
            var vNormalOrthDbls = vNormalOrthSw.ArrayData;

            var surf = (Surface) modeler.CreatePlanarSurface2(centerDbls, vNormalDbls, vNormalOrthDbls);


            var startPoint = center + radius*vRef.Unit();
            var endPoint = center - radius*vRef.Unit();

            var startPointDbls = startPoint.ToDoubles();
            var endPointDbls = endPoint.ToDoubles();

            
            var arco = modeler.CreateArc
                        (centerDbls, vNormalDbls, radius, startPointDbls, endPointDbls);

            var arc = (Curve) arco;
            var arcStartPoint = startPoint;
            var arcEndPoint = endPoint;

            var trimmedArc = arc.CreateTrimmedCurve2(arcStartPoint.X,arcStartPoint.Y,arcStartPoint.Z,arcEndPoint.X,arcEndPoint.Y,arcEndPoint.Z);
            var line = modeler.CreateTrimmedLine(arcEndPoint, arcStartPoint);
            return (IBody2) surf.CreateTrimmedSheet(new[] {trimmedArc, line});
        }
Ejemplo n.º 12
0
        public static Vector4 FromAxisAngle(Vector3 axis, float angle)
        {
            if (axis.Dot() == 0.0f)
                return Identity;

            Vector4 result = Identity;

            angle *= 0.5f;
            axis.Normalize();
            result._x = axis._x * (float)System.Math.Sin(angle);
            result._y = axis._y * (float)System.Math.Sin(angle);
            result._z = axis._z * (float)System.Math.Sin(angle);
            result._w = (float)System.Math.Cos(angle);

            return result.Normalize();
        }
Ejemplo n.º 13
0
        public static bool LinePlaneIntersect(Vector3 lineStart, Vector3 lineEnd, Vector3 planePoint, Vector3 planeNormal, out Vector3 result)
        {
            Vector3 diff = lineEnd - lineStart;
            float scale = -planeNormal.Dot(lineStart - planePoint) / planeNormal.Dot(diff);

            if (float.IsNaN(scale) || scale < 0.0f || scale > 1.0f)
            {
                result = new Vector3();
                return false;
            }

            result = lineStart + (diff * scale);
            return true;
        }
Ejemplo n.º 14
0
        public void dot() {
            var a = new Vector3(5, 2, -1);
            var b = new Vector3(3, -3, 0);

            Assert.Equal(9, a.Dot(b));
            Assert.Equal(9, b.Dot(a));
        }
Ejemplo n.º 15
0
        /// ラインと三角形との衝突
        public bool CheckTriangle( DemoGame.GeometryLine moveLine, ShapeTriangles trgShape )
        {
            Vector3 collPos = new Vector3(0,0,0);

            calMovePos = moveLine.EndPos;
            calBestDis = -1.0f;
            calBestId  = -1;

            float checDis = moveLine.Length + collCheckDis;

            for( int i=0; i<trgShape.EntryNum; i++ ){

            float a = (calMovePos.Dot( trgShape.Triangle[i].Plane.Nor ) + trgShape.Triangle[i].Plane.D);
            if( a >= checDis || a <= -checDis ){
                continue;
            }

            if( DemoGame.CommonCollision.CheckLineAndTriangle( moveLine, trgShape.Triangle[i], ref collPos ) == true ){
                float dis = Common.VectorUtil.Distance( collPos, moveLine.StartPos );
                if( dis < calBestDis || calBestId < 0 ){
                    calMovePos        = collPos;
                    calBestDis        = dis;
                    calBestId        = i;
                }
            }
            }

            if( calBestId >= 0 ){
            return true;
            }
            return false;
        }
Ejemplo n.º 16
0
        /// 点と線分の最近接点を算出
        public static float GetClosestPtPosRay( Vector3 trgPos, Vector3 trgRay, Vector3 trgRayPos, ref Vector3 cPos )
        {
            Vector3 ab, ca;
            float a_t;
            float a_dotCA;
            float a_dotAA;

            ab = trgRay;
            ca = trgPos - trgRayPos;

            a_dotCA = ca.Dot( ab );
            a_dotAA = ab.Dot( ab );

            if( a_dotAA <= epsilon ){
            a_t = 0.0f;
            }
            else{
            a_t = (a_dotCA / a_dotAA);
            }
            //		if( a_t < epsilon ){
            //			a_t = 0.0f;
            //		}
            //		else if( a_t > 1.0f ){
            //			a_t = 1.0f;
            //		}

            /// 最近接点の算出
            cPos.X = trgRayPos.X + ( a_t * ab.X );
            cPos.Y = trgRayPos.Y + ( a_t * ab.Y );
            cPos.Z = trgRayPos.Z + ( a_t * ab.Z );

            /// 最近接点と点との距離を返す
            Vector3 calVec = trgPos - cPos;
            float dis = FMath.Sqrt( calVec.Dot(calVec) );

            return dis;
        }
Ejemplo n.º 17
0
        public static bool LineSphereIntersect(Vector3 start, Vector3 end, Vector3 center, float radius, out Vector3 result)
        {
            Vector3 diff = end - start;
            float a = diff.Dot();

            if (a > 0.0f)
            {
                float b = 2 * diff.Dot(start - center);
                float c = (center.Dot() + start.Dot()) - (2 * center.Dot(start)) - (radius * radius);

                float magnitude = (b * b) - (4 * a * c);

                if (magnitude >= 0.0f)
                {
                    magnitude = (float)Math.Sqrt(magnitude);
                    a *= 2;

                    float scale = (-b + magnitude) / a;
                    float dist2 = (-b - magnitude) / a;

                    if (dist2 < scale)
                        scale = dist2;

                    result = start + (diff * scale);
                    return true;
                }
            }

            result = new Vector3();
            return false;
        }
Ejemplo n.º 18
0
        private Ray GetRefractionRay(Vector3 P, Vector3 N, Vector3 V, double refraction)
        {
#if true
            return new Ray(P, V, Ray.sameSurfaceOffset, double.MaxValue);
#else
            //V = V * -1;
            //double n = -0.55; // refraction constant for now
            //if (n < 0 || n > 1) return new Ray(P, V); // no refraction

            double c1 = N.Dot(V);
            double c2 = 1 - refraction * refraction * (1 - c1 * c1);
            if (c2 < 0)


                c2 = Math.Sqrt(c2);
            Vector3D T = (N * (refraction * c1 - c2) - V * refraction) * -1;
            T.Normalize();

            return new Ray(P, T); // no refraction
#endif
        }
        public static void Test_Vector3_Generic_Dot_Product_Against_Unity3D(float a, float b, float c, float d, float e, float f)
        {
            //arrange
            Vector3<float> genericVec3One = new Vector3<float>(a, b, c);
            Vector3<float> genericVec3Two = new Vector3<float>(d, e, f);

            UnityEngine.Vector3 unityVec3One = new UnityEngine.Vector3(a, b, c);
            UnityEngine.Vector3 unityVec3Two = new UnityEngine.Vector3(d, e, f);

            //act
            float genericVec3Dot = genericVec3One.Dot(genericVec3Two);
            float unityVec3Dot = UnityEngine.Vector3.Dot(unityVec3One, unityVec3Two);

            //assert
            Assert.AreEqual(genericVec3Dot, unityVec3Dot); //this is done with Operator so it doesn't get JIT I think so it's less accurate. We need kEpsilon for it.
        }
Ejemplo n.º 20
0
        protected bool TrySnapToSurface(MyGridPlacementSettings.SnapMode snapMode)
        {
            if (m_closestHitDistSq < float.MaxValue)
            {
                Vector3 newDragPointPosition = m_hitPos;

                bool isAnyStatic = AnyCopiedGridIsStatic;
                if (isAnyStatic)
                {
                    m_pasteDirForward = Vector3.Forward;
                    m_pasteDirUp = Vector3.Up;
                }
                else if (m_hitNormal.Length() > 0.5)
                {
                    if (snapMode == MyGridPlacementSettings.SnapMode.OneFreeAxis)
                    {
                        m_pasteDirUp = m_hitNormal;

                        float dotUp = m_pasteDirUp.Dot(m_hitEntity.WorldMatrix.Up);
                        float dotFwd = m_pasteDirUp.Dot(m_hitEntity.WorldMatrix.Forward);
                        float dotRt = m_pasteDirUp.Dot(m_hitEntity.WorldMatrix.Right);
                        if (Math.Abs(dotUp) > Math.Abs(dotFwd))
                        {
                            if (Math.Abs(dotUp) > Math.Abs(dotRt))
                            {
                                m_pasteDirForward = Vector3.Normalize(m_hitEntity.WorldMatrix.Right - (dotRt * m_pasteDirUp));
                            }
                            else
                            {
                                m_pasteDirForward = Vector3.Normalize(m_hitEntity.WorldMatrix.Forward - (dotFwd * m_pasteDirUp));
                            }
                        }
                        else
                        {
                            if (Math.Abs(dotFwd) > Math.Abs(dotRt))
                            {
                                m_pasteDirForward = Vector3.Normalize(m_hitEntity.WorldMatrix.Up - (dotUp * m_pasteDirUp));
                            }
                            else
                            {
                                m_pasteDirForward = Vector3.Normalize(m_hitEntity.WorldMatrix.Forward - (dotFwd * m_pasteDirUp));
                            }
                        }
                    }
                    else if (snapMode == MyGridPlacementSettings.SnapMode.Base6Directions)
                    {
                        var hitGrid = m_hitEntity as MyCubeGrid;
                        if (hitGrid != null)
                        {
                            Matrix hitGridRotation = hitGrid.WorldMatrix.GetOrientation();
                            Matrix firstRotation = GetFirstGridOrientationMatrix();
                            Matrix newFirstRotation = Matrix.AlignRotationToAxes(ref firstRotation, ref hitGridRotation);
                            Matrix rotationDelta = Matrix.Invert(firstRotation) * newFirstRotation;

                            m_pasteDirForward = newFirstRotation.Forward;
                            m_pasteDirUp = newFirstRotation.Up;
                            m_pasteOrientationAngle = 0.0f;
                        }
                    }
                }

                Matrix newOrientation = GetFirstGridOrientationMatrix();
                Vector3 globalCenterDelta = Vector3.TransformNormal(m_dragPointToPositionLocal, newOrientation);

                m_pastePosition = newDragPointPosition + globalCenterDelta;

                if (MyDebugDrawSettings.DEBUG_DRAW_COPY_PASTE)
                {
                    MyRenderProxy.DebugDrawSphere(newDragPointPosition, 0.08f, Color.Red.ToVector3(), 1.0f, false);
                    MyRenderProxy.DebugDrawSphere(m_pastePosition, 0.08f, Color.Red.ToVector3(), 1.0f, false);
                }

                IsSnapped = true;
                return true;
            }
            IsSnapped = false;
            return false;
        }
Ejemplo n.º 21
0
 public void Assign(Vector3 n, Vector3 pos)
 {
     Normal = n.Normalized();
     Dot = Normal.Dot(pos);
 }
Ejemplo n.º 22
0
        static void half_diff_coords_to_std_coords( double _ThetaHalf, double _PhiHalf, double _ThetaDiff, double _PhiDiff,
													ref Vector3 _In, ref Vector3 _Out )
        {
            double	SinTheta_half = Math.Sin( _ThetaHalf );
            Half.Set( Math.Cos( _PhiHalf ) * SinTheta_half, Math.Sin( _PhiHalf ) * SinTheta_half, Math.Cos( _ThetaHalf ) );

            // Build the 2 vectors representing the frame in which we can use the diff angles
            Vector3	OrthoX;
            Half.Cross( ref Normal, out OrthoX );
            if ( OrthoX.LengthSq() < 1e-6 )
                OrthoX.Set( 1, 0, 0 );
            else
                OrthoX.Normalize();

            Vector3	OrthoY;
            Half.Cross( ref OrthoX, out OrthoY );

            // Rotate using diff angles to retrieve incoming direction
            Half.Rotate( ref OrthoX, -_ThetaDiff, out Temp );
            Temp.Rotate( ref Half, _PhiDiff, out _In );

            // ...or by mirroring in "Half tangent space"
            double	MirrorX = -_In.Dot( ref OrthoX );
            double	MirrorY = -_In.Dot( ref OrthoY );
            double	z = _In.Dot( ref Half );
            _Out.Set(
                MirrorX*OrthoX.x + MirrorY*OrthoY.x + z*Half.x,
                MirrorX*OrthoX.y + MirrorY*OrthoY.y + z*Half.y,
                MirrorX*OrthoX.z + MirrorY*OrthoY.z + z*Half.z
            );

            // 			if ( _In.z < -0.5 || _Out.z < -0.5 )
            // 				throw new Exception( "RHA MAIS MERDE!" );
        }
        public static IBody2 CreateCirclularSheet
            ( this IModeler modeler
            , Vector3 center
            , Vector3 vNormal
            , Vector3 vRef
            , double radius)
        {
            // Should be orthogonal
            Debug.Assert(vRef.Dot(vNormal)<1e-9);

            var math = SwAddinBase.Active.Math;
            var centerSw = center.ToSwMathPoint();
            var vNormalSw = vNormal.ToSwMathPoint();
            var vNormalOrthSw = vRef.ToSWVector(math).Normalise();

            var centerDbls = centerSw.ArrayData;
            var vNormalDbls = vNormalSw.ArrayData;
            var vNormalOrthDbls = vNormalOrthSw.ArrayData;

            var surf = (Surface) modeler.CreatePlanarSurface2(centerDbls, vNormalDbls, vNormalOrthDbls);


            var startPoint = centerSw.AddTs(vNormalOrthSw.ScaleTs(radius));

            var startPointDbls = startPoint.ArrayData;

            
            var arco = modeler.CreateArc
                        (centerDbls, vNormalDbls, radius, startPointDbls, startPointDbls);

            var arc = (Curve) arco;
            var arcStartPoint = arc.StartPoint();
            var arcEndPoint = arc.EndPoint();

            var trimmedArc = arc.CreateTrimmedCurve2(arcStartPoint.X,arcStartPoint.Y,arcStartPoint.Z,arcEndPoint.X,arcEndPoint.Y,arcEndPoint.Z);
            return (IBody2) surf.CreateTrimmedSheet(new[] {trimmedArc});
        }
Ejemplo n.º 24
0
 public float DistanceToPoint(Vector3 point)
 {
     var v = new Vector3(Normal);
     return v.Dot(point) + Constant;
 }
Ejemplo n.º 25
0
		public static JacobianContact GetDOF(
			int indexA,
			int indexB,
			Vector3 linearComponentA,
			Vector3 linearComponentB,
			Vector3 angularComponentA,
			Vector3 angularComponentB,
			SimulationObject simulationObjectA,
			SimulationObject simulationObjectB,
			double constraintValue,
			double correctionValue,
			double cfm,
			double constraintLimit,
			ConstraintType type,
            	int? contactReference = null,
			StartImpulseProperties startImpulseProperties = null)
		{
			double jacobianVelocityValue = linearComponentA.Dot (simulationObjectA.LinearVelocity) +
			                               linearComponentB.Dot (simulationObjectB.LinearVelocity) +
			                               angularComponentA.Dot (simulationObjectA.AngularVelocity) +
			                               angularComponentB.Dot (simulationObjectB.AngularVelocity);

			jacobianVelocityValue -= constraintValue;

			if (startImpulseProperties == null)
				startImpulseProperties = new StartImpulseProperties(0.0);

			return new JacobianContact (
				indexA,
				indexB,
				contactReference,
				linearComponentA,
				linearComponentB,
				angularComponentA,
				angularComponentB,
				type,
				jacobianVelocityValue,
				correctionValue,
				cfm,
				constraintLimit,
				startImpulseProperties);
		}
Ejemplo n.º 26
0
 public void Assign(Vector3 v1, Vector3 v2, Vector3 pos)
 {
     Normal = v1.Cross(v2);
     Normal = Normal.SafeNormalize();
     Dot = Normal.Dot(pos);
 }
Ejemplo n.º 27
0
		private static JacobianContact[] addFriction(
			SimulationObject objectA,
			SimulationObject objectB,
			SimulationParameters simulationParameters,
			int indexA,
			int indexB,
			Vector3 normal,
			Vector3 relativeVelocity,
			Vector3 ra,
			Vector3 rb,
			List<StartImpulseProperties> startImpulseProperties)
		{
			JacobianContact[] friction = new JacobianContact[2];

			var tx = new Vector3 ();
			var ty = new Vector3 ();

			GeometryUtilities.ComputeBasis(
				normal,
				ref tx,
				ref ty);

			double constraintLimit = 0.0;

            Vector3 tangentialVelocity = relativeVelocity -
                                         (normal.Dot(relativeVelocity)) *
                                         normal;

            #region Get start friction direction

            if (Vector3.Length (tangentialVelocity) >
				simulationParameters.ShiftToStaticFrictionTolerance) 
				constraintLimit = 0.5 * (objectA.DynamicFrictionCoeff + objectB.DynamicFrictionCoeff);
			else 
				constraintLimit = 0.5 * (objectA.StaticFrictionCoeff + objectB.StaticFrictionCoeff);
			
			#endregion

			#region Tangential Direction 1

			var linearComponentA = tx;
			var linearComponentB = -1.0 * linearComponentA;

			var angularComponentA = ra.Cross (linearComponentA);
			var angularComponentB = -1.0 * rb.Cross (linearComponentA);

			friction [0] = JacobianCommon.GetDOF (
				indexA,
				indexB,
				linearComponentA,
				linearComponentB,
				angularComponentA,
				angularComponentB,
				objectA,
				objectB,
				0.0,
				0.0,
				simulationParameters.FrictionCFM,
				constraintLimit,
				ConstraintType.Friction,
                	null,
				startImpulseProperties[1]);

			#endregion

			#region Tangential Direction 2

			linearComponentA = ty;
			linearComponentB = -1.0 * linearComponentA;

			angularComponentA = ra.Cross (linearComponentA);
			angularComponentB = -1.0 * rb.Cross (linearComponentA);

			friction [1] = JacobianCommon.GetDOF (
				indexA,
				indexB,
				linearComponentA,
				linearComponentB,
				angularComponentA,
				angularComponentB,
				objectA,
				objectB,
				0.0,
				0.0,
				simulationParameters.FrictionCFM,
				constraintLimit,
				ConstraintType.Friction,
                null,
				startImpulseProperties[2]);

			#endregion

			return friction;
		}