Beispiel #1
0
        virtual public void adjustStepHeight()
        {
            Vector3 footPos = lowLCon.getSwingFoot().getWorldBottomPosition();
            //Vector3 swingFootV = lowLCon.getSwingFoot().getAbsoluteVelocityForLocalPoint(lowLCon.getSwingFoot().getLocalBottomPosition());
            Vector3 swingFootV = lowLCon.getCharacterFrame() * (lowLCon.getVelSagittal() * lowLCon.getCharacter().getLocalFrontAxis() - lowLCon.getVelDCoronal() * lowLCon.getCharacter().getLocalLeftAxis());

            swingFootV[lowLCon.getWorldUpAxisID()] = 0;
            //swingFootV.Normalize();
            Vector3 rayPos      = footPos + 0.7f * swingFootV;
            float   rayStartY   = lowLCon.getComPosition() [lowLCon.getWorldUpAxisID()];
            Vector3 rayStartPos = rayPos;

            rayStartPos[lowLCon.getWorldUpAxisID()] = rayStartY;
            float   rayEndY   = (footPos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis) [lowLCon.getWorldUpAxisID()];
            Vector3 rayEndPos = rayPos;

            rayEndPos[lowLCon.getWorldUpAxisID()] = rayEndY;

            RaycastHit raycastHit = new RaycastHit();

            if (Physics.Linecast(rayStartPos, rayEndPos, out raycastHit, stepLayerMask))
            {
                Debug.DrawLine(rayStartPos, rayEndPos, Color.blue);
                lowLCon.swingFootFrontPosition = raycastHit.point;
            }
            else
            {
                Vector3 pos = lowLCon.getSwingHip().getChildJointWorldPosition() - lowLCon.getLegLength() * SimGlobals.upAxis;
                Debug.DrawLine(pos, pos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis, Color.blue);
                lowLCon.swingFootFrontPosition = pos - lowLCon.additionHeight * SimGlobals.upAxis;
            }
            //Debug.Log("aaaaaaaaaaaaa  "+lowLCon.swingFootFrontPosition);

            rayPos      = footPos;
            rayStartPos = rayPos;
            rayStartPos[lowLCon.getWorldUpAxisID()] = rayStartY;
            rayEndPos = rayPos;
            rayEndPos [lowLCon.getWorldUpAxisID()] = (footPos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis) [lowLCon.getWorldUpAxisID()];
            if (Physics.Linecast(rayStartPos, rayEndPos, out raycastHit, stepLayerMask))
            {
                Debug.DrawLine(rayStartPos, rayEndPos, Color.green);
                lowLCon.swingFootBasePosition = raycastHit.point;
                lowLCon.swingFootBaseNormal   = raycastHit.normal;
            }
            else
            {
                lowLCon.swingFootBasePosition = lowLCon.swingFootFrontPosition;
                lowLCon.swingFootBaseNormal   = Vector3.zero;
            }
            //Debug.Log("bbbbbbbbbbbbb  "+lowLCon.swingFootBasePosition);
            //	Debug.Log("aaaaaaaaaaaaaaaaa  "+lowLCon.swingFootBaseNormal);

            footPos     = lowLCon.getStanceFoot().getWorldBottomPosition();
            rayPos      = footPos;
            rayEndY     = (footPos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis) [lowLCon.getWorldUpAxisID()];
            rayStartPos = rayPos;
            rayStartPos[lowLCon.getWorldUpAxisID()] = rayStartY;
            rayEndPos = rayPos;
            rayEndPos[lowLCon.getWorldUpAxisID()] = rayEndY;
            if (Physics.Linecast(rayStartPos, rayEndPos, out raycastHit, stepLayerMask))
            {
                Debug.DrawLine(rayStartPos, rayEndPos, Color.green);
                lowLCon.standFootBasePosition = raycastHit.point;
                lowLCon.standFootBaseNormal   = raycastHit.normal;
            }
            else
            {
                Vector3 pos = lowLCon.getStandHip().getChildJointWorldPosition() - lowLCon.getLegLength() * SimGlobals.upAxis;
                Debug.DrawLine(pos, pos - (2 * maxStepHeight + lowLCon.additionHeight) * SimGlobals.upAxis, Color.green);
                lowLCon.standFootBasePosition = pos - lowLCon.additionHeight * SimGlobals.upAxis;
                lowLCon.standFootBaseNormal   = Vector3.zero;
            }
            //	Debug.Log("bbbbbbbbbbbbbbbbb  "+lowLCon.standFootBaseNormal);

            lowLCon.swingToStandBaseVector = Quaternion.Inverse(lowLCon.getCharacterFrame()) * (lowLCon.standFootBasePosition - lowLCon.swingFootBasePosition);
            //Debug.Log("bbbbbbbbbbbbb  "+lowLCon.swingToStandBaseVector);
            float h = lowLCon.getDoubleStanceCOMError().magnitude * 1.6f;

            h = Mathf.Clamp(h, lowLCon.getStepHeight(), maxStepHeight);
            lowLCon.swingLegPanicHeight = h - lowLCon.getStepHeight();
            //Debug.Log("bbbbbbbbbbbbbbbbb "+lowLCon.swingLegPanicHeight);
        }